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1.0 INTRODUCTION 


The Space Station Mechanism Test Bed is a six degree of freedom (DOF) 
motion simulation facility used to evaluate docking and berthing hardware 
mechanisms. The major components of the Mechanism Test Bed (MTB) are shown 
in Figure 1.0-1. The chase vehicle docking mechanism is mounted on the 
hydraulically driven, computer controlled, six DOF motion system. The target 
vehicle docking mechanism is mounted in conjunction with a force and moment 
sensor to the facility ceiling. Mechanism contact forces and moments are measured 
and supplied to the host computer (i.e., currently, an Alliant computer) for use in the 
dynamics model. 

Under contract NAS8-36570, Control Dynamics (CDy) developed a 
generalized rigid body math model to replace the "old" model which was based on 
several restrictive assumptions (e.g., one body was assumed to have much greater 
mass that the second and therefore was unaffected by contact forces and moments). 
The "new" model allowed the computation of vehicle relative motion in six DOF due 
to forces and moments from mechanism contact, attitude control systems, and 
gravity. No vehicle size limitations were imposed in the model. The equations of 
motions were based on Hill’s equations for translational motion with respect to a 
nominal circular earth orbit and Newton-Euler equations for rotational motion. Over 
the past several years, CDy has worked with NASA in refining this rigid body model 
and the supporting software. 

This report documents the development of a generalized flexible body math 
model to further enhance the MTB simulation capabilities. Although the original 
contract plan was to modify the current rigid body model, early investigations 
showed that a "fresh start" approach to the flex body model would yield a more 
efficient simulation. The development and major components of the flex body math 
model parallel those of the rigid body model. 
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CONTACT DYNAMICS USING 0 DOF MOTION SYSTEM 

Figure 1.0-1 














Section 2.0 of this report summarizes the rather mathematically intense 
derivation of the equations of motion for a single generic flexible body. The 
derivation is based on Lagrange's quasi-coordinate equations. Section 3.0 
describes the method used to transform contact forces and moments from the 
sensor location to each body docking port. Section 4.0 discusses the computation 
of the relative body motion data: (1) relative orientation, (2) relative position, (3) 
relative translational velocity, and (4) relative angular velocity. This data is required 
in the interface between the dynamic math model and the main simulation. Section 
5.0 describes the major components of the math model FORTRAN simulation 
including the required input data and a pre-processing algorithm for flexible body 
data. The model was coded with user selectable options regarding the method of 
integration and the complexity of the equations of motion. These options are 
discussed in Section 5.0 along with the governing input data. Section 6.0 
discusses the many tests used to verify the flex body math model in progressive 
steps. Time domain comparisons to the multi-flex body code called TREETOPS are 
also presented. Section 7.0 contains a brief summary with concluding remarks. 
Appendices A and B contain a listing of the flexible body math model code and 
definitions for the math model global simulation variables, respectively. Finally, 
Appendix C contains a listing of the flexible body data pre-processing algorithm. 
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2.0 FLEXIBLE BODY MODEL 

Table 2.0-1 lists the most popular methods, along with advantages and 
disadvantages, for deriving equations of motion (EOM) of dynamical systems. The 
D'Alembert / Newton-Euler equations are best suited for rigid body applications; for 
complex systems, the form of the equations depend to a large extent on the insight 
of the analyst. The other methods (i.e., Lagrange’s Eqns. and Kane's Eqns.) offer a 
more systematic approach. 

Lagrange’s generalized coordinate equations are represented by the vector 
equation 




(2.0-1) 


where L = kinetic energy - potential energy (Lagrangian) 
q = vector of generalized coordinates 
Q = vector of generalized forces. 


This "standard" form of Lagrange’s equations is well known and produces EOM with 
the same basic structure for all applications. A limitation of Lagrange's "standard" 
equations is that the generalized coordinate vector derivative (4) must be integrable 
to a set of coordinates (q) which completely describe the system configuration. 
Specifically, this precludes the use of body angular rates (©) in the q vector (i.e., the 
integral of {co dt} does not adequately describe body attitude). This limitation can be 
overcome by employing a modified form of Lagrange’s equations known as 
Lagrange's quasi-coordinate equations or the Boltzmann-Hamel equations. Quasi- 
coordinates (w) are quantities whose differentials may be written as linear 
combinations of generalized coordinate derivatives, e.g., 
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Table 2.0-1 Candidate Methods for Equations of Motion Derivation. 
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( 2 . 0 - 2 ) 


£(wi) = a i^(q0 + a 2^(q2) + - 

Thus, the quasi-coordinate form of Lagrange’s equations allows the direct use of 
body angular rates in the EOM coordinate vector which replaces the q coordinate 
vector (i.e., (q, q, t) -> (w, q, t) ). Lagrange's quasi-coordinate method was used to 
derive the EOM for the MTB generic flexible body model. 

Kane’s equations combines certain advantages from D'Alembert's principle 
and Lagrange's method. The multi-flex body code TREETOPS is based on Kane's 
equations. However, Kane’s method is not as well known as the Lagrangian 
formulation. 

2.1 Lagrange's Quasi-Coordinate Equations 

The quasi-coordinate vector definition used in the flex body model is 

f P B 
W=j g,b 

1 A 

where P B = translational velocity of undeformed body CM with re- 
spect to (wrt) inertial space expressed in the body frame 

to 6 = angular velocity of undeformed body frame wrt inertial 
space expressed in the body frame 

A = time derivative of the generalized modal 
coordinate vector. 

The associated generalized coordinate vector was defined using the four Euler 
parameters (i.e., a quaternion) for attitude 
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( 2 . 1 - 2 ) 



The transformation from the quasi-coordinate vector to the derivative of the 
generalized coordinate vector can be expressed as 

[B l] T 0 0 

0 ^E t 0 

0 0 I 

where [B l] T = 3x3 body to inertial transformation matrix 

(1/2) E T = 4x3 transformation from the body angular velocity 

vector to the quaternion time derivative 

£4 *£3 

E t = £3 £4 

-£2 £1 

_ -El -£2 
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1 = 3x3 identity matrix. 


Starting with Lagrange's "standard" equations (2.0-1), Lagrange's quasi-coordinate 
equations can be derived for the coordinate vector definitions given in eqns. (2.1-1) 
through (2.1-4). The EOM separate nicely into vector equations for translation, 
rotation, and flexibility. The resulting equations can be expressed in general as 


Translational EOM: 



+ <o D x 


9T 

0P 


B 


r , 0T r n av 

- [bi] + [bi] = [bo qp> 


9P 


9P 


(2.1-5) 


Rotational EOM: 


d, 

dt 



+ co B 



+ P 


B 



2 C '5e 


c av' 


h* 


( 2 . 1 - 6 ) 


Flexibility EOM: 


0T \ 3T 9V* 

'dt\) ' + 11 (2.1-7) 

where T = kinetic energy function in terms of quasi-coordinates 

V* = potential energy function augmented with constraint 
equations. 



Notice that application of Lagrange’s equations requires a derivation of both kinetic 
and potential energy functions (scalars) for the system under consideration. 

Constraints are included with the potential energy function using the 
Lagrange multiplier technique to form an augmented potential energy function 
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V* = V(q) - X, T <J)(q) 


( 2 . 1 - 8 ) 


where X, is a vector of Lagrange multipliers and the constraints are of the form 

<J> (q) = 0. (2.1-9) 

For an unconstrained flexible body (as considered here), the only constraint 
equations will be those resulting from the use of dependent coordinates (e.g., 
quaternion for attitude). 


2.2 Flexible Body Kinetic Energy Function 

The kinetic energy function for a flexible body can be derived using a mass 
particle approach. Using the configuration definitions given in Fig. 2.2-1, the inertial 
position of the i th mass particle of a generic flexible body may be written as 

ri = R 0 + pi + 8i (2.2-1) 


Taking the time derivative of the mass particle position vector with respect to the 
inertial frame leads to 

r-i = R 0 + co x (pi + 80 + (8^ (2.2-2) 

where (Site is a derivative wrt the body fixed frame. 


The total kinetic energy (i.e., translational plus rotational contributions) for a body 
may be written as 


T = 2 X [ m i( f i • f i) + (® + ®i) T (® + ®i)] 

i 


(2.2-3) 
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mj = i* h mass particle 

O = reference point on flexible body 

rj = mass particle inertial position vector 

Ro = body reference point inertial position vector 

Pi = mass particle nominal position vector wrt O 

5j = translational deformation 

0j = rotational deformation 

co = "rigid body" angular velocity vector 


Figure 2.2-1 Generic Flexible Body Configuration. 


The somewhat lengthy derivation proceeds by substituting eqn. (2.2-2) into eqn. 
(2.2-3) and manipulating the kinetic energy function into a useful form. The 
translational and rotational elastic deformation vectors for the i th mass particle are 
computed from modal expansions as given in eqn. (2.2-4). 

#modes #modes 

§i = ®ij “H j ®i = ^ij "Hj 

j j (2.2-4) 

The translational and rotational mode shapes (<D and X P, respectively) are 
commonly obtained from a finite element model (FEM) of the body. The resulting 
kinetic energy function is written in "shorthand" notation as 
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T = |MR® 2 + ^ T MT1 + R®- 

nm nm 

+ Cl) • X Z ( r ik +r2jk Tlj lik) 
j k 


(, 


nm 


tDX Mp + X TokTlk 
V k > 


ILlll 

+ R? • X ( r ok Tlk) 

k 


nm nm 

+ j (0 T X X (^jk'Hj Tlk + 2 Ilk "Hk + lo) CO 
j k 


(2.2-5) 


where r ok and r 1k are the "standard" modal integrals and r 2 j k , lik. and l 2 j k are 
called the higher order modal integrals. The modal integral terms are functions of 
the mode shapes, FEM element masses, and FEM element inertias. The modal 
integrals are constants and appear in the final form of the equations of motion; they 
are typically computed from the FEM output data for use as inputs to a flex body 
dynamic simulation. 


2.3 Flexible Body Potential Energy Function 

The potential energy function for a flexible body may be assumed to consist 
only of the strain energy due to linear elastic deformation. In this case, all other 
conservative forces which are derivable from potential functions (e.g., gravity 
effects) must be applied as forcing functions (i.e., external or generalized forces) on 
the right hand side of the EOM. 

The strain energy in a linear elastic material may be expressed in the form 


V=1?K5 

2 


(2.3-1) 


where 8 is the deformation due to flexibility and K is the FEM stiffness matrix. Here 
8 represents both the translational and rotational deformation. Replacing 8 in eqn. 
(2.3-1) with a modal expansion yields 

V = (2.3-2) 
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where the modal matrix <D includes both the translational and rotational modal 
gains. If the mode shapes, are the system othonormal eigenvectors, then a new 
diagonalized stiffness matrix can be defined 


K = O'KO 


co? 0 0 

0 co? 0 

0 0 cog 


(2.3-3) 


where the diagonal elements are the squared natural frequencies of the system 
modes. The assumption of othonormal modes follows from NASA's request in the 
statement of work for the capability to input second order bending modes of the form 

rji + 2 Ci C0i tii + co? Tji = Fi. (2.3-4) 


The potential energy or strain energy may be written in terms of the diagonalized 
stiffness matrix 


V = 1t|TKti 


(2.3-5) 


Notice that the potential energy function is a function of the generalized modal 
coordinates T|. 

The augmented potential energy function used in Lagrange's quasi- 
coordinates eqns. (2.1-5) through (2.1-7) included constraints with Lagrange 
multipliers. The only constraint equation associated with the unconstrained single 
flex body model is the unity magnitude quaternion constraint. The augmented 
potential energy function can now be written as 

V* = V(q) - X T <|)(q) 
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where Xi is the Lagrange multiplier. 


2.4 Equations of Motion 

The kinetic energy function (2.2-5) and the augmented potential energy 
function (2.3-6) may now be incorporated into Lagrange's quasi-coordinate 
equations (2.1-5) through (2.1-7). The body reference point (i.e., point O in Fig. 2.2- 
1) is assumed to be located at the body center of mass in order to simplify the 
equations somewhat. After lengthy algebraic manipulations, the "full-up" nonlinear 
EOM can be expressed in the following "shorthand" notation 


M 

-r 0 Ti 

r 0 


Pcm 1 

roil 

TT 

r 1 + r 2 

< 

cbB 

r T 
1 0 

ri T + r 2 T 

M 

; 

1 *1 


= Fl + Fnl + Fappl 


APPL 


G) B X (M PcM + 2 To Tl) - CD B X (CD® X To T| ) + 2 


APPL l 

- II G) B - CD B X II CD B - T 0 T1 X (CD b x p B M ) . qjB x ( Fl + T 2 ) H + 2 T " 


APPL „ APPL 


CD 


- 2CkCD k T^ + Pcm • (0> B X T 0 ) + 2 < F m + 2 

m 
nm 

2 Ilr + 2 (^2jr + l2rj) "Hj 


- 2 co B • t 2 + i-to® 7 
2 


CD 


n 

B 



where 


parts = number of mass particles 

appl = number of externally applied forces or torques. 


M = 


MOO 
0 M 0 
LOOM 


parts 

M = X 



Mi 

0 ■ 

<Di 

w 

0 

Ii J 

w 


To = [r 0 l r 02 — Ton] 


H = [I'll r 12 — Tin] 


r 2 = £nj[r 2 ji r 2j2 ... r 2 j n ] 

j 

Oi = [®ii O i2 ... 


nm 


r 2 = 2 nj [ r 2ji r 2j2 - r 2jn ] 

j 

▼i = [fil ^12 - YjJ 


II = IcM + (Ilk + Ilk T ) ^lk 2 ^ 2 J k + 


II = (Ii k + Ilk T ) ilk + 2 ^ 2 J k + * 2k ^ ^k + 2 ^ 2jk + l2k ^ (2 4-2) 


"Standard" Modal Integrals: 


parts 

rok = X n^ik 

i 


parts 

rik = X (mi pi x O ik + Ii^ik) 

i 


(2.4-3) 


Higher Order Modal Integrals: 

parts 

r 2j k = X mi Oij x Oik 

i 

parts 

Ilk = X mi[(Pi-*ik)l - ^ikPi 1 )] 

i 

parts 

I 2 jk = X m i [(^ij T <&*) 1 - ^ij^ik 7 )] (2 4 4) 
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where 


parts = number of mass particles. 


Notice that for the "full-up" nonlinear case the mass matrix has time varying 
elements; this implies that periodic updates of the time varying elements are 
required in the flexible body math model. 

Examination of the "full-up" nonlinear EOM reveals that the equations can be 
written in levels of increasing simplicity by neglecting certain higher order effects. 
First, neglecting the higher order modal integral terms allows the EOM to be written 
as 


M -r 0 Ti To 

To Tl 1 cm r 1 

r 0 T rV m 


PcM 


CD 

It 


B 


APPL \ 

- C0 B X (M Pcm + 2 r 0 T)) - C0 B X (G) b X To Tl) + X F m 


APPL 


cd b xIcmCO b - r 0 T| x (cd b x Pcm) - ^xOTjil) + X T B 


APPL APPL 

- COiTl - 2Ck(B k Tl + Pcm • (0) B X r 0 ) + X Fm + X ^ T I 


(2.4-5) 


Eliminating the higher order modal integrals reduces the computations per 
simulation cycle. Notice in eqn. (2.4-5) that if the "standard" modal integrals were 
zero (i.e., free-free modes assumption discussed below), then neglecting the higher 
order modal integrals would eliminate the time varying terms from the mass matrix; 
this would allow a "one-time" computation of the mass matrix in the initialization and 
a significant decrease in simulation cycle time. 
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Secondly, elimination of the remaining nonlinear terms (i.e., includes 
elimination of the higher order modal integrals) on the right hand side of the EOM 
leads to another level of simplification 


M 

-iVn 

To 


Pcm| 

fvn 

IcM 

r i 


d> B 

r T 

L A o 

Ti T 

M 


i *1 ) 


APPL \ 

X Fm 

m 

APPL 

X Tg 

n 

APPL APPL 

, - Cfl£ TJ - 2Ck<0kH + X °mFm + X ^ I 

m n 


( 2 . 4 - 6 ) 


Eliminating the nonlinear terms on the right hand side greatly reduces the 
computational requirements per simulation cycle and again decreases cycle time. 

Another simplification level results from the use of free-free modes for the 
unconstrained flexible body. The "standard" modal integral terms are identically 
zero when free-free modes are used. The result is a significant simplification of the 
mass matrix (and the right hand side if the previous simplification was not invoked) 


' M 

0 

0 


i*CM | 

0 

Icm 

0 

( 

cb B 

0 

0 

M 

! 

k ) 


APPL 

X 

m 

APPL 

X T n 

n 

APPL 


APPL 


- 2CkC0kt) + X ^f! + X 


( 2 . 4 - 7 ) 
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The first and second simplifications resulting in eqns (2.4-5) and (2.4-6) are not 
prerequisites for applying the free-free mode simplification. However, using the 
free-free mode simplification in conjunction with eliminating the higher order modal 
integral terms produces a constant mass matrix which can be computed once in the 
initialization. Notice that the translational, rotational, and flexibility equations are 
completely decoupled with respect to one another when the following 
simplifications are applied: (1) neglect higher order mass integral terms, (2) 
neglect remaining right hand side nonlinear terms, and (3) utilize free-free modes. 
NOTE: It is important to invoke the free-free mode option explicitly in the math 
model configuration data when free-free modes are being used to characterize 
body flexibility. Invoking the free-free option eliminates unnecessary computations 
that will otherwise be performed with standard modal integrals which are zero (or 
very small due to numerical rounding). 

A final simplification of the EOM results when no flex modes are included 
(i.e., rigid body). In this case, the EOM reduce to the well known linear form of the 
Newton-Euler equations for translation and rotation 


M 

0 



(2.4-8) 


All of the above mentioned EOM complexity levels are made available to the 
user of the two-flex body math model. The user will configure the math model to the 
desired complexity level through the use of flags and parameters in ASCII input 
data files. 
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2.5 


Regardless of the level of complexity, the equations of motion can be 
expressed in the general form 

A • x = b (2.5-1) 

where A = system mass matrix 

x = quasi-coordinate vector derivative 
b = right hand side vector. 

The mass matrix (A) and the right hand side vector (b) are computed (or known) at 
each point in time. The objective is to solve the linear equation set for the quasi- 
coordinate vector derivative (x) such that numerical integration can be performed to 
propagate the system states. 

Recall that depending on the EOM complexity options selected by the user, 
the system mass matrix may or may not vary with time. If the mass matrix is not time 
varying (i.e., constant elements) then a "one-time" solution approach to the linear 
equation set of (2.5-1) is desired. This would allow a portion of the computations 
associated with solving (2.5-1) to be performed once in an initialization routine and, 
thus, reduce the math model cycle time. 

A numerical manipulation method called LU (lower-upper) decomposition 
was selected for solving eqn. (2.5-1). The LU decomposition decomposes the mass 
matrix into a product of a lower triangular matrix (L) and an upper triangular matrix 
(U). 

A ■ x = (L ■ U) x = L • (U • x) = b 


an 

0 

0 ' 

r pn 

Pl2 

Pin 

a 2 i 

a 22 

0 

u = 0 

P 22 

P2n 

. a n i 

®n 2 

ttnn - 

L o 

0 

Pnn . 


134.0691.FH 


18 


(2.5-2) 



The decomposition is based on a numerical manipulation technique called Crout's 
algorithm. Once the mass matrix has been decomposed, the problem of solving for 
the desired vector x is broken into two smaller linear equation problems which can 
be solved using backwards and forwards substitution 

L • y = b (forward) (2.5-3) 

U • x = y (backward) (2.5-4) 

Eqn. (2.5-3) can be solved using forward substitution since L is lower triangular; 
eqn. (2.5-4) can be solved using backward substitution since U is upper triangular. 

Notice that the LU decomposition of the mass matrix can be used to solve for 
x with any right hand side b. Thus, for a constant mass matrix the LU 
decomposition needs only to be computed once during initialization; given a new 
right hand side vector b during each simulation cycle, only the forward/backward 
substitution operations associated with eqns. (2.5-3) and (2.5-4) are required to 
solve for x. Another possible solution method is to compute the mass matrix 
inverse A* 1 . For the constant mass matrix case only a matrix-vector multiplication 
(given the computed b vector) would be required during each cycle to solve for x 

x = A ' 1 b. (2.5-4) 

However, for the time varying mass matrix case the LU decomposition (performed 
each cycle) is numerically more efficient than computing the matrix inverse. 


134.0691.FR 


19 


3.0 CONTACT FORCE / MOMENT TRANSFORMATION 

During hardware-in-the-loop (HWIL) simulations, the math model requires 
input from the force/moment (F/M) sensor located on the ceiling of the MTB six DOF 
facility. The sensor location is assumed to correspond to some arbitrary position on 
the target body. Transformations are required in order to determine the appropriate 
force/moment applied to the docking node of each body. The body docking node is 
defined as the single point on the body through which all contact forces and 
moments are applied to the body. 

The F/M sensor measures the total forces and torques applied to the target 
body due to hardware contact between the target and chase bodies. It is assumed 
that a rigid link exists in the target body between the F/M sensor and the docking 
mechanism; therefore, all contact forces and moments, from all points of contact, 
are detected by the F/M sensor. 

It is also assumed that the docking mechanism mass (for each body) is much 
smaller than the body masses. This assumption implies that the inertial forces 
acting on the moving parts of the docking mechanisms may be neglected. In this 
case, the contact forces and moments between the two bodies are equal and 
opposite for collocated docking nodes. 

Figure 3.0-1 defines the position vectors from the F/M sensor to the target 
docking node (D1) and to the chase docking node (D2). Due to the rigid link 
assumption, PD1S1 remains constant in a frame attached to either the sensor 
location or the target docking node. However, PD2S1 includes the effects of 
deformation of both the sensor location (or Di since they are rigidly connected) and 
the chase docking node. Fsi and Msi are defined as the sensed force and 
moment. F D1. Mqi, Fd2 and Md2 are defined as the contact force and moment 
applied at the target docking node and chase docking node, respectively. 
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Figure 3.0-1 F/M Sensor to Docking Node Position Vector Definitions. 

Due to the rigid link assumption, the forces can be transferred directly from 
the sensor location to the docking nodes as 

FD1 =-FD2 = Fsi (3.0-1) 

The moment transformations include the effect of non collocated docking nodes and 
are expressed as 

MD1 = Msi - PD1S1 x FD1 (3.0-2) 

Md 2 = - Msi - PD2S1 x Fq2 (3.0-3) 

Notice that if the docking nodes are collocated (i.e., PD2S1 = PD1S1) the contact 
moments are also equal and opposite. 
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4.0 RELATIVE BODY MOTION 

The role of the two-flex body math model is to simulate the dynamic response 
of the two bodies when acted upon by contact, control, and any other modeled 
external forces and torques. The most important output from the math model is the 
relative motion between the target and chase docking nodes. This data is used to 
generate the commands which drive the six DOF motion system. 

The transformation matrix describing the orientation of the chase docking 
frame with respect to the target docking frame must include the effects of flexibility. 
This is most easily expressed by defining undeformed node frames (i.e., rigid body 
node frames) for use as intermediate transform orientations. Following this 
approach, the 3 x 3 transformation matrix from the D2 to the D1 node frame may be 
written as 

[D1 D2] = [I D1] T [I D2] (4.0-1) 

= [D1 0 D1] T [B1 D1 o] T [I B1] T [I B2] [B2 D2 0 ] [D2 0 D2] 

where Dio and D2o are undeformed docking node coordinate frames and I 
represents the inertial reference frame. The transformation from the deformed node 
frame to the undeformed node frame can be expressed in terms of the infinitesimal 
rotations about each axis 

[Dio Dl] = [R Xl de, Ry,d© y Rz.d©*] = [l + dQoi] (4.0-2) 

Figure 4.0-1 defines the vectors which are used in the equations describing 
the relative motion. The vectors representing deformation are not shown explicitly 
in Fig. 4.0-1. The position of the chase docking node with respect to the target 
docking node may be expressed as 
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PD2D1 = Pf32l + PD2oB2 + 8d2D2o " PbII “ Pd1 0 B1 ' SdIDIo (4.0-3) 

where the 8 vectors represent the effects of flexibility. The result is a vector and may 
be expressed in any coordinate frame. 



Figure 4.0-1 Relative Body Position Vector Definitions. 

The relative velocity between docking nodes is defined as the velocity of the 
chase docking node as seen by an observer moving with the target docking node. 
Therefore, the derivative is taken wrt the target docking node frame. Equation (4.0- 
4) is the well known relationship for the time derivative of a vector viewed from two 
frames rotating at different angular rates. 

(R)a = (R)b + ©ba x R (4.0-4) 

where (R)a = time derivative of an arbitrary vector R wrt A frame 
(R)b = time derivative of the vector R wrt B frame 
©ba = angular velocity of B frame wrt A frame. 
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Using the vector definitions in Fig. 4.0-1 and applying eqn. (4.0-4) results in the 
expression for the time derivative of the position of D2 wrt D1 taken wrt the D1 
frame. 
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(4.0-5) 


The deformation vectors are represented by their respective modal expansions in this 
expression. 

The relative angular velocity of the chase docking frame wrt the target docking 
frame is given in eqn. (4.0-6). 
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Again, the rotational deformation vectors are expressed as a modal expansion. 
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5.0 SIMULATION DESCRIPTION 

The computer code which implements the MTB flexible body math model is 
coded in FORTRAN 77 and all floating point numbers are maintained with double 
precision accuracy. A listing of the computer code, along with input data files, is 
contained in Appendix A. The math model is coded in a modular form to facilitate 
debugging and modifications. The math model is controlled by the driver 
subroutine "DYNAMIC" which is called by the host simulation. This driver 
subroutine and the common declarations in the include file "INTFAC.INC" serve as 
the interface to the host simulation. 

The following nomenclature is used for specific data extracted from 
coordinate vectors. Position vectors begin with the letter P. Translational velocity 
vectors begin with the letters PD. The vector P_A_B_C is defined as the position 
vector from point B to point A expressed in the C frame coordinates; PD_A_B_C is 
the time derivative of P_A_B_C. Angular velocity vectors begin with the letter W and 
derivatives follow the pattern described above. Transformation matrices begin with 
the letter T. The transformation T_A_B is a 3 x 3 matrix which transforms vectors 
from the B frame to the A frame. The labels used to define bodies, docking nodes, 
etc., in Figures 3.0-1 and 4.0-1 are also used in the simulation code. 

5.1 Major Subroutines 

Figure 5.1-1 is a flow diagram of the MTB two flex body math model. A 
general description of each of the major subroutines and include files is given in 
Table 5.1-1 and Table 5.1-2, respectively. The first pass through the model 
initializes the simulation variables. Data from the force/moment sensor is supplied 
to the subroutine "FMTRANS" through common declarations in the include file 
"INTFAC.INC". The relative motion data is made available to the host simulation 
through common declarations in the include file "INTFAC.INC". 
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Subroutine "DYNAMIC' 


Driver of two-flex 
body math model 


F/M 

Sensor 



Call Subroutine "FMTRANS" 
Call Subroutine "CONTROL (1)" 
Call Subroutine "CONTROL (2)" 



Initialization 


Call Subroutine "IN IT" 

Call Subroutine "INTFAC" 

Call Subroutine "FMTRANS" 

Call Subroutine "CONTROL (1)" 
Call Subroutine "CONTROL (2)" 
Call Subroutine "LUMASSM (1)" 
Call Subroutine "LUMASSM (2)" 
Call Subroutine "PLANT (1)" 

Call Subroutine "PLANT (2)" 


Target 

Time Varying Mass' 
N. Matrix? 


Call Subroutine "LUMASSM (1)” 



Chase 

Time Varying Mas^ 
n. Matrix? / 


Call Subroutine "LUMASSM (2)” 


Call Subroutine "PLANT (1)" 
Call Subroutine "PLANT (2)" 
Call Subroutine "INTEG (1)" 
Call Subroutine "INTEG (2)" 
Call Subroutine "INTFAC" 


Relative 

Motion 


Return 


Figure 5.1-1 Two Flexible Body Math Model Flow Diagram. 
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Table 5.1-1 Two Flex Body Math Model Subroutines. 


Subroutine 

Description 

DYNAMIC 

Driver of two-flex body dynamics model; called by main routine. 

INIT 

Initialization of mass properties, geometry, state vectors, 
flexibility data, etc. 

FMTRANS 

Computes contact forces/torques at docking port nodes based 
on force/torque sensor data. 

CONTROL (NB) 

Computes control forces/torques applied to NBltL body. 

LUMASSM (NB) 

Computes mass matrix of NBlil body in lower-upper 
decomposed form. 

PLANT (NB) 

Computes right hand side terms (e.g., damping, stiffness, 
forcing functions, etc.) of equations of motion and solves A x=b 
for NBUl body. 

INTEG (NB) 

Integrates derivatives of quasi and generalized coordinate 
vectors and updates generalized coordinate vector derivative 
for NBitl body. 

INTFAC 

Updates relative transformations, position vectors and rate 
vectors with flexibility included. 


Table 5.1-2 Two Flex Body Simulation Include Files. 


INCLUDE FILE DESCRIPTION 

BODY.INC Variable/parameter declarations and common specifications for 

mass properties, coordinate vectors, flexibility, etc. for both 
bodies; also simulation control variables. 


INTFAC.INC Variable declarations and common specifications for interfacing 
to main routine; includes all necessary relative motion data. 
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5.2 


The MTB flex body simulation has three ASCII input data files. Table 5.2-1 
gives a general description of the contents of these input data files. A computer 
listing of example input data files is contained in Appendix A. 

The ASCII input data may be modified using any available editor (e.g., the VI 
editor on the UNIX based ALLIANT computer system). The existing input files 
contain comments (see example files in Appendix A) which help to define the input 
data. Most of the input data is well described by the comments. The EOM 
complexity options are defined with the following acronyms: 

Description 

Eliminate higher order modal integral terms 

Eliminate remaining right hand side nonlinear terms 
Note: Enabling ENRLT encompasses EHOMIT regardless 
of the EHOMIT option status. 

Free-Free modes 

The EOM complexity options are declared as logical variables. Therefore, ".T." is 
TRUE and enables the option; ".F." is FALSE and disables the option. 

The pre-processed flex data (see Table 5.2-1) forms the bulk of the required 
flexibility data and is generated by the flexible body data pre-processing algorithm 
discussed in Section 5.3. The remaining body data can be generated using a file 
editor and then concatenated with the output of the pre-processing algorithm to 
produce the whole input data file. 

Currently, all the input data specified in Table 5.2-1 is read regardless of the 
EOM options. For example, enabling the EHOM-IT option does not eliminate the 
need for higher order modalmtegral data in the respective body input data file even 
though the data is not used in the simulation. This follows from the fact that the 
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Table 5.2-1 Two Flex Body ASCII Input Data Files. 


INPUT FILE 
MTBSIM.INP 


BODY1.INP 


BODY2.INP 


DESCRIPTION 

Simulation control data: Integration step size 

Screen output step size 
Data tile output step size 
Stop time 

Integration method flag 

Target body data: Mass 

Inertia matrix 

Initial P_BJ_I 

Initial PD_B_M 

Initial w_B_l_B 

[I B] Euler rotation sequence 

[I B] Euler rotation angles 

Number of nodes; dock node num 

Node geometry 

*P_D1_S1_S1 

[B1 Dio] Transformation 

*[D1 SI] Transformation 

EOM complexity options 

Flexibility Data: Number of flexible modes 

Mode frequencies 
Modal damping 

Initial generalized modal coords. 
Initial gen modal coord derivatives 


Pre-Processed Generalized mass matrix 

Flex Data: Translational modal gains 

Rotational modal gains 
Standard modal integrals 
Higher order modal integrals 


Chase body data: Same as target body data omitting * 
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flexible body data pre-processing routine (Section 5.3) computes and outputs all 
modal integral terms. An advantage of this approach is that the input data files (for a 
given body) have a "standard" structure (i.e., same contents). 

5.3 Flexible Bodv Data Pre- Processing Algorithm 

Flexible body simulations require a great deal more input data than pure rigid 
body simulations. Typically, the simulation input data (e.g., modal integrals and 
generalized mass matrix) is computed from the output of a finite element model 
(FEM). To assist users of the MTB flexible body simulation, Control Dynamics 
coded a flexible body data pre-processing algorithm. This algorithm acts as a post- 
processor for FEM data and a pre-processor for the MTB flexible body simulation. 
Figure 5.3-1 is a block diagram which illustrates the role of the algorithm. 



Figure 5.3-1 Flexible Body Data Pre-Processing Algorithm. 
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Table 5.3-1 describes the flexible body data required in the ASCII input file for the 
pre-processing algorithm. This data is read using free format (see pre-processor 
code listing in Appendix C). The output (see flexibility data in Table 5.2-1) is written 
to the user specified file in a format consistent with the inputs to the MTB math 
model simulation. The output file from the pre-processor contains the data denoted 
as pre-processed flex data in Table 5.2-1. 


Table 5.3-1 Flexible Body Data Pre-Processor Input. 


Input Description 
Degree of Freedom Map 

Node Geometry Table 
Mass Matrix 


Explicit Input Data 

# nodes, # DOF, # modes 
Blank line 

node num (i), DOF type (i) 
Blank line 
‘Offset vector 
Node position vector (i) 
Blank line 

Mass_mat (# DOF, # DOF) 
Blank line 

Phi (# DOF, # modes) 


Modal Matrix 

*A displacement vector subtracted from each node position vector 
which allows the body reference point to be relocated. 
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6.0 SIMULATION VERIFICATION 

The math model simulation was verified in a series of progressive steps on a 
PC-386 computer at Control Dynamics Company prior to installation on the host 
computer . This section of the report documents the various tests used to verify the 
math model simulation. Time domain results for all of the various simulation 
component tests are not presented; only the results from an "all-encompassing" two 
flexible body comparison to the multi-flex body code TREETOPS is shown. The 
"full-up" flexible body verification run presented in section 6.4 was repeated after the 
model was installed on the host computer to ensure proper operation of the model 
in the MTB facility. This "full-up" run exercises all aspects of the simulation and, 
therefore, eliminates the need to reverify the simulation in a component manner on 
the host computer. 

6.1 Equations of Motion Solution Verification 

The LU decomposition technique used to solve for the derivative of the quasi- 
coordinate vector is described in Section 2.5 of this report. Since the LU 
decomposition and forward/backward substitution algorithms were coded to solve a 
general linear set of equations A x = b (i.e., the symmetry property of the mass 
matrix was not considered), the algorithm may be verified using any arbitrary 
deterministic system with a square matrix A. The LU decomposition algorithms 
were used to solve several linear systems of various size. The same systems were 
then solved using the well established computer analysis tool called MATLAB. The 
solution vectors (x) were compared to ensure proper operation of the EOM solution 
technique. 
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6.2 Rigid Body Verification 

The rigid body portion of the math model was tested by two means. First, 
each degree of freedom (i.e., 3 translations and 3 rotations per body) was tested 
independently through the application of a constant force/moment. The results were 
verified by hand calculations. 

Secondly, the simple two rigid body system shown in Figure 6.2-1 was used 
to test portions of the relative vehicle motion calculations and the contact 
force/moment transformation. For the system in Fig. 6.2-1, an omnidirectional spring 
of stiffness K and undeformed length r was assumed at the target docking node in 
order to simulate contact forces. Various initial conditions on the chase vehicle 
were used to induce contact between the docking ports in each translational axis. 
The resulting rigid body motion was verified through hand calculations. Also, forces 
and torques representing contact were input as "sensed" values to simulate data 
from the F/M sensor in the MTB facility. The math model operated on these 
"sensed" values to produce contact forces and torques at the body docking nodes. 
Again, the results were validated using hand calculations. 



Figure 6.2-1 Two Rigid Body Contact Test Case. 
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6.3 


The flexible body math model was tested using a free-free flexible beam 
model created in the structural computer code SMIS (Structures and Matrix 
Interpretive System). The beam geometry and properties are shown in Figure 6.3-1 . 



Node 4 


Length = 24.78 m 
Radius = 0.1 m 

Density = 8410 kg/m 2 32.74 0 0 

Elasticity = 100 xIO 9 Nt/m 2 ICM = 0 335K 0 kg - m 2 

Rigidity = 30 x 10 2 Nt/m 2 0 0 335K 

Figure 6.3-1 Properties of Free-Free Flexible Test Beam. 

The beam was sized to yield a first bending mode near 1 Hz. Table 6.3-1 
lists the first four flexible mode frequencies from the SMIS model of the beam 
described in Figure 6.3-1. 
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Table 6.3-1 First Four Bending Modes of Free-Free Flexible Test Beam. 


Mode Number 

Frequency (Hzl 

Description 

1 

1.003 

Z Translation/Y Rotation 

2 

1.003 

Y Translation/Z Rotation 

3 

2.770 

Z Translation/Y Rotation 

4 

2.770 

Y Translation/Z Rotation 


The SMIS flexible beam model (i.e., mass matrix, mode shapes, DOF map, 
node geometry, etc.) was processed by the flexible body data processing algorithm 
discussed in Section 5.3 The processor produced the generalized mass matrix, 
modal gains, and modal integrals required for the MTB flexible body input data files 
(See Section 5.2). 

6.4 Flexible Bodv Verification 

The flexible beam model was implemented in two other flexible body 
simulations for comparisons to the MTB math model. Table 6.4-1 summarizes the 
important differences among the simulations. First, the model was implemented in 
an in-house (CDy) flex body simulation based on Lagrange's quasi-coordinate 
EOM (see Section 2.0), but coded independently. This simulation comparison 
facilitated rapid debugging since the terms in the equations of motion had a "one-to- 
one" correspondence. Secondly, the flexible beam model was implemented in the 
multi-flex body simulation called TREETOPS. TREETOPS is based on Kane's 
equations and gives a totally independent verification of the MTB math model. The 
majority of the data in TREETOPS is maintained as single precision. Notice also 
(from Table 6.4-1) that both test simulations use a different integration method than 
the MTB simulation. Only a comparison between the MTB math model and 
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TREETOPS is presented in this report. 


Table 6.4-1 Simulation Comparison. 



CDy In-House 
TREETOPS 


Double 

Double 

Single 



HWIL Schemes 


4-Pass Runge-Kutta 
4-Pass Runge-Kutta 


Several flexible body test cases were investigated using different "contact" 
forcing functions and body configurations. Only one "full-up" two flexible body 
comparison to TREETOPS is presented here; this run exercises "all" features of the 
MTB flexible body math model. 

Figure 6.4-1 illustrates the system configuration and contact force/moment 
application points for the flexible body verification run. Equations (6.4-1) and (6.4-2) 
define the "contact" forcing function applied to the target docking node (D1 ). Notice 


V 



Z 


Figure 6.4-1 Two Flexible Beam Test Case. 
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t > 0.02 sec. 


(6.4-1) 


(6.4-2) 


that the "contact" forcing function is applied over a short time period (impulsive 
loading) which is characteristic of actual contact. 

Figure 6.4-2 is a TREETOPS block representation of the two flexible beam 
test case shown in Figure 6.4-1 with the forcing functions defined in eqns. (6.4-1) 
and (6.4-2). The function generators are step functions which produce the forcing 
functions; position and negative step functions are used to "turn-on" and "turn-off" 
the forces and moments. Actuators 1,2,3, and 4 are reaction jets which apply a 
force at a given node; these actuators implement the equal and opposite "contact" 
forces at the body docking nodes. Actuators 5 and 6 are moments actuators which 
implement the equal and opposite "contact" moments. In TREETOPS, the axis of 
application of these actuators changes in the body fixed frame (i.e., undeformed 
body frame) due to body flexing effects. Body 1 and 2 are both three mode flexible 
models of the beam shown in Figure 6.3-1. Hinges 1 and 2 are configured with zero 
translational and rotational stiffness to allow the bodies to respond as "free-floaters" 
(i.e., unconstrained bodies). Sensor 1 is an inertial position vector sensor which 
outputs the position of the chase docking node (D2) with respect to the target 
docking node (D1). Sensor 2 is an inertial velocity sensor which outputs the 
velocity of D2 with respect to D1. The TREETOPS simulation used Runge-Kutta 
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integration with a step size of 0.01 seconds; data was also written out every 0.01 
seconds. 



AC = Actuator HI = Hinge 

BO = Body IN = Interconnect 

FU = Function Generator SE = Sensor 

Figure 6.4-2 TREETOPS Block Representation of Two Flexible Beam Test Case. 

The "contact" forcing function defined in eqns (6.4-1) and (6.4-2) was 
implemented in the MTB math model by computing an equivalent "sensed" force 
and moment (i.e., at some arbitrary F/M sensor location on the target body). This 
"sensed" force and moment was then used as if the data came from a physical 
sensing device; the "sensed" force and moment was transformed to the appropriate 
body docking nodes and applied as an external forcing function. The MTB was 
initialized such that the docking nodes were collocated at time zero. Referring to 
Section 3.0 of this report, the "contact" forces will be equal and opposite as in the 
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TREETOPS simulation. However, the "contact" moments will be equal and opposite 
only for collocated docking nodes; the TREETOPS simulation of this two flexible 
beam problem will apply equal and opposite "contact" moments over the entire 
duration of the forcing function. The results should still be comparable since the 
"contact" forcing function is of short duration; thus, the relative displacement of the 
docking nodes should be small during the forcing period. 

In the MTB simulation, both the target and chase bodies are three mode 
flexible models of the beam shown in Figure 6.3-1. The modal damping was zero 
and the higher order modal integral terms were neglected; TREETOPS has no 
direct way of including non-zero modal damping and requires the use of a COSMIC 
NASTRAN FEM model to include higher order modal integrals. The MTB simulation 
was configured to use Adams (3-1 )/2 integrations with a step size of 0.002 seconds. 
The smaller step size (factor of 5 lower than TREETOPS step size) should give 
results comparable to the multi-pass Runge-Kutta scheme. 

The results of the MTB and TREETOPS simulation runs will now be 
presented. Since both bodies are based on the same generic flexible body model, 
the presented data focuses on the target body and the relative motion data. 

Figure 6.4-3 shows the time response of the "contact" forcing functions 
applied to the target docking node. These correspond to equations (6.4-1) and (6.4- 
2). Figure 6.4-4 contains both the MTB and TREETOPS results for the target body 
center of mass (CM) velocity vector expressed in the inertial frame. The 
corresponding curves are plotted with the same line style and the plot scale does 
not allow the observer to differentiate between comparable curves. Figure 6.4-5 
shows the vector difference between the TREETOPS and the MTB velocity vectors 
for the target body. The remaining data will be presented in the format of Figures 
6.4-4 and 6.4-5; that is, the comparable MTB and TREETOPS data (usually a 3 
element vector) will be plotted on the same graph at the top of the page and the 
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vector difference (TREETOPS - MTB) will be shown in the next figure at the bottom 
of the page. 

Figures 6.4-6 and 6.4-7 contain results for the target body CM position 
expressed in the inertial frame. Figures 6.4-8 and 6.4-9 show the target body 
angular velocity vector data. There is significant difference between the MTB and 
the TREETOPS results for the angular velocity about the body X-axis. The moment 
arm associated with the torque about the X-axis which produces this angular 
velocity arises solely from body flexing. The "contact" moment about the body y-axis 
displaces the target docking node in the negative Z direction. The "contact" force 
along the positive Y direction, in conjunction with the Z displacement, produces the 
torque about the X-axis. Investigations into the different results showed that the X- 
axis angular velocity is sensitive to modal damping; this follows since the moment 
arises solely from flexibility. In turn, the X-axis angular velocity is also sensitive to 
the method of integration; different numerical integration methods induce different 
levels of numerical damping. For example, repeating the MTB simulation run using 
rectangular integration produces an X-axis angular velocity value much closer to 
the TREETOPS result. The conclusion is that the difference in integration methods, 
combined with the effects of single versus double precision and the sensitivity of the 
X-axis angular velocity in this particular case, is the source of the differing results. 

Figures 6.4-10 and 6.4-11 contain the generalized modal coordinate 
derivative data for the target body. Figures 6.4-12 and 6.4-13 show the generalized 
modal coordinate data for the target body. Figures 6.4-14 and 6.4-15 show the 
translational deformation data associated with the target docking node. This data is 
expressed in the target body frame and represents deformation with respect to the 
rigid body. Figures 6.4-16 and 6.4-17 contain data for the rotational deformation of 
the target docking node with respect to the undeformed position. 
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Figures 6.4-18 and 6.4-19 show the results for the relative translational 
velocity of the chase docking node with respect to the target docking node 
expressed in the inertial frame. Figures 6.4-20 and 6.4-21 contain data for the 
relative docking node position vector (D2 with respect to D1), expressed in the 
inertial frame. Notice in general that the difference between the MTB and 
TREETOPS results appears to be growing with increasing time. This can be 
attributed to the difference in numerical damping associated with the respective 
integration techniques and the effects of single versus double precision simulation 

variables. 


MTB_TT D1 "Contact" Forcing: FN4=(-3K,2K,0,0,15K,0) 0<=t<0.02; 



Figure 6.4-3 Target Docking Node "Contact" Forcing Functions. 
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B1 P_B_I_I (TT-MTB) (m) 



Time (sec) 


Figure 6.4-7 TREETOPS - MTB Target Body CM Position. 
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B 1 W_B_I_I (TT-MTB) (rad/sec) B 1 W_B_I_I (rad/sec) 


xlO -3 MTB_TT B 1 Ang Vel: FN4=(-3K,2K,0,0,15K,0) 0<=t<0.02 



Figure 6.4-8 Target Body Angular Velocity. 


xlO -5 MTB_TT B 1 Ang Vel Diff: FN4=(-3K,2K,0,0,15K,0) 0<=t<0.02 



Figure 6.4-9 TREETOPS - MTB Target Body Angular Velocity. 
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MTB_TT Dock Rel Velocity: FN4=(-3K,2K,0,0,15K,0) 0<=t<0.02 



Figure 6.4-18 Docking Node Relative Translational Velocity. 


xlO' 3 MTB_TT Dock Rel Vel Diff: FN4=(-3K,2K,0,0,15K,0) 0<=t<0.02 



Figure 6.4-19 TREETOPS - MTB Docking Node Relative Translational Velocity. 
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Diff: FN4=(-3K,2K,0,0,15K,0) 0<=t<0.02 



7.0 SUMMARY AND CONCLUSIONS 

Per the contract objective, a generalized flexible body math model has been 
developed to further enhance the MTB Facility capabilities. The math model 
assumes that the flexible modes which characterize body bending will be 
orthonormal modes. This assumption allows the bending modes to be 
characterized by the standard second order equation 

iii + 2CiO)iTii + co? rii = Fi 

as requested in the statement of work. Only minor modifications to the math model 
would be required to handle other less common mode types (e.g., non-orthogonal 
modes which typically result from sub-structure coupling techniques such as Craig 
Bampton component mode synthesis). 

The model was coded in FORTRAN 77 utilizing double precision floating 
point numbers. The math model code offers the user several levels of complexity for 
the equations of motion and three hardware-in-the-loop numerical integration 
options. Due to the increased volume of data over pure rigid body runs, a flexible 
body data pre-processing algorithm has been coded to generate simulation input 
data from FEM output data. The code has been installed and verified on the Alliant 
computer in the MTB facility. 

In the future, the flexible body math model with be incorporated into the host 
simulation. The cycle times associated with various configurations (i.e., various 
EOM complexity levels) will need to be determined. Modifications to the flexible 
body math model may be required in order to reduce the simulation cycle time. 
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APPENDIX A 


MATH MODEL SIMULATION CODE 


ASCII Input File : MTBSIM.INP 

0.002, 500, 5 Integ step size (sec); dt per CRT ; dt per write 
2. IStop time 

2 IINT: 1=Euler (Adamsl), 2=Adams3-1, 3=Adams23-16+5 


ASCII Input File : BODY1.INP 


6547. 

32.74, 0., 0. 

0., 335000., 0. 
0., 0., 335000. 
0 ., 0 ., 0 . 

0 ., 0 ., 0 . 

0 ., 0 ., 0 . 

1,2,3 

0 ., 0 ., 0 . 

4,4 

-12.39,0., 0. 
-4.13, 0.,0. 
4.13, 0., 0. 
2.39, 0., 0. 

0., 0., 0. 
1 .. 0 .. 0 . 
0..1..0. 

0.,0., 1. 

1 .. 0 .. 0 . 

0., 1., 0. 

0.,0., 1. 

.T.,.F.,.T. 


! mass (c-u); Free-Free Brass Beam Model 
! inertia matrix (c-u); symmetric 


! P B I I (length c-u); SAME AS GSV(1-3) 

! PD_B_I_B (rate c-u); SAME AS QSV(1-3) 

! W_B_I_B (deg/sec); SAME AS QSV(4-6) 

! I <-- BODY: Euler rot seq (1=x, 2=y, 3=z) 

! I <-- BODY: Euler angles (deg) 

! # nodes, NODE_DOCK; BODY origin at CM 
! node 1 in BODY 
! node 2 in BODY 
! node 3 in BODY 
! node 4 
I P_D1_S1_S1 
! T_B1_D10 


IT D1 SI 


! EOM: 1)T=EHOMIT; 2)T=(1)+ERNLT 3)T=FREE-FREE 


3 I # modes 

6.2993, 6.2993, 17.4063 ! modal freqs (rad/sec) 

0.0, 0.0, 0.0 ! modal damping (zetaj) 

0., 0., 0. modal coords: eta(NM); GSV(>7) 

0 ., 0., 0. I deriv modal coords: etadot(NM); QSV(>6) 

1.000000128348988 0.000000000E+000 -6.931 270607381 940E-01 8 IGen mass 


0.00000000E+000 1.000000128348988 O.OOOOOOOOOOOOOE+OOO 

-5.1 431 840557281 12E-01 7 0.0000000E+000 1.000000170771713 

■OOOOOOOE+OO .0000000E+00 .2485285E-01 ! Translational Modal Gains: Phi 
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.OOOOOOOE+OO 

.OOOOOOOE+OO 

.OOOOOOOE+OO 

.OOOOOOOE+OO 

.OOOOOOOE+OO 

.OOOOOOOE+OO 

.OOOOOOOE+OO 


.2485285E-01 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
-.921 6621 E-02 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
-.9216621 E-02 


.OOOOOOOE+OO 
.2462543E-01 
-.921 6621 E-02 


.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
-.1 00071 5E+01 
.OOOOOOOE+OO 
.1 00071 5E+01 
.OOOOOOOE+OO 
.5551115E-16 
.OOOOOOOE+OO 
-.5551115E-16 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.475261 OE+OO 
.1 00071 5E+01 
.OOOOOOOE+OO 


.OOOOOOOE+OO 
.OOOOOOOE+OO 
.2485285E-01 
.OOOOOOOE+OO 
.46631 53E-02 
.OOOOOOOE+OO 
.7881236E-02 
.2742354E-02 
.OOOOOOOE+OO 
-.1 159555E-02 
-.2742354E-02 
.OOOOOOOE+OO 
-.1 159555E-02 
-.46631 53E-02 
.OOOOOOOE+OO 
.7881236E-02 
.OOOOOOOE+OO 
-.5443560E-05 
.OOOOOOOE+OO 
-.355271 4E-1 3 
.OOOOOOOE+OO 
-.21 19259E-04 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.1 00071 5E+01 


.OOOOOOOE+OO 
-.1633864E-01 
-.921 6621 E-02 
.OOOOOOOE+OO 
.1633864E-01 
.2485285E-01 
.OOOOOOOE+OO 
-.2462543E-01 
.OOOOOOOE+OO 
-.46631 53E-02 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
-.2742354E-02 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.2742354E-02 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.46631 53E-02 
.OOOOOOOE+OO 
-.5443560E-05 
.OOOOOOOE+OO 
.355271 4E-1 4 
.OOOOOOOE+OO 
.355271 4E-1 3 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 


! Rotational Modal Gains: Ps 


! Gamma_0 


! Gamma_1 


! Gamma_2jk 


II Ik 


! I_2jk 
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.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.5551115E-16 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.1 00071 5E+01 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
-.2775558E-16 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.1013732E+01 
.OOOOOOOE+OO 
.OOOOOOOE+OO 


.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
-.1 00071 5E+01 
.OOOOOOOE+OO 
.5551 1 15E-16 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
-.2775558E-16 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.2775558E-16 
.OOOOOOOE+OO 
.1013732E+01 
.OOOOOOOE+OO 


.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
-.1 00071 5E+01 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.1 00071 5E+01 
.OOOOOOOE+OO 
-.5551115E-16 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 
.OOOOOOOE+OO 


ASCII Input File : B0DY2.INP 


Same structure as B0DY1.INP omitting P_D1_S1_S1 and T_D1_S1 data 
since the F/M sensor is located only on the target body. 
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X 


cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


cc 

cc Filename: 
cc 

cc Function: 


cc 

cc 

cc Source: 
cc 

cc Comments: 
cc 


BODY.INC 

Include file for variable/common declarations 
relating to body data. 

JC Date: 2/91 

Double precision 


cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


INTEGER MNM IMAX # OF MODES PER BODY 

INTEGER MNN IMAX # OF NODES PER BODY 

INTEGER NBODES I# OF BODIES 

PARAMETER (MNM = 10, MNN = 10, NBODES = 2) 

INTEGER NODE_DOCK(NBODES) '.BODY DOCKING PORT NODE NUM 
INTEGER NMODES(NBODES) !# FLEX MODES PER BODY 
INTEGER NNODES(NBODES) I# NODES PER BODY 

INTEGER INDX(NBODES,6+MNM) !REC OF LUDCMP ROW PERMUTATIONS 


cc Simulation options vars 

LOGICAL*2 EOM(NBODES,3) 
INTEGER INT 
REAL*8 TIME 
REAL*8 DT 


IOPTIONS FOR EOM COMPLEXITY 
! INTEGRATION OPTIONS 
ISIM TIME (SEC) 

(INTEGRATION STEP SIZE (SEC) 


cc Mass properties/modal data vars 
REAL*8 MASS(NBODES) 

REAL*8 ICM(NBODES,3,3) 

REAL*8 GMASS(NBODES,MNM,MNM) 
REAL*8 NODES(NBODES,MNN,3) 


IBODY MASS 

IINERTIA MAT ABOUT BODY CM 
IGEN MASS MATRIX PER BODY 
INODE LOCATIONS PER BODY IN 
! BODY 


REAL*8 MOD_FREQ(NBODES,MNM) 
REAL*8 MOD_ZETA(NBODES,MNM) 
REAL*8 PHI(NBODES,MNN,MNM,3) 
REAL*8 PSI(NBODES,MNN,MNM,3) 

REAL*8 PHIETA(NBODES,MNN,3) 
REAL*8 PHIETAD(NBODES,MNN,3) 
REAL*8 PSIETA(NBODES,MNN,3) 
REAL*8 PSIETAD(NBODES,MNN,3) 


IMODAL FREQUENCIES PER BODY 
IMODAL DAMPING PER BODY 
ITRANS MODAL GAINS 
IROT MODAL GAINS 

ISUM OF PHI*ETA PER NODE 
ISUM OF PHFETAD PER NODE 
ISUM OF PSFETA PER NODE 
ISUM OF PSFETAD PER NODE 


cc Mass Integral Terms 

REAL*8 GAM_0(NBODES,3,MNM) IARR OF GAM_0 MASS INTEGS 

REAL*8 GAM_1 (NBODES, 3, MNM) IARR OF GAM_1 MASS INTEGS 

REAL*8 GAM_2JK(NBODES,MNM,MNM,3) IARR OF GAM_2JK MASS INTS 
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REAL*8 l_1 K(NBODES,MNM,3,3) !ARR OF 3X3 l_1 K MASS INTS 

REAL*8 l_2JK(NBODES,MNM,MNM,3,3) !ARR OF 3X3 l_2JK MASS INTS 

REAL*8 LU MASSM(NBODES,6+MNM,6+MNM) IBODY #NB LUDCMP 

! MASS MAT 


cc External body forces/moments 

REAL*8 FN_BODY(NBODES,MNN,3) FORCES APPLIED AT EACH NODE 
REAL*8 MN_BODY(NBODES,MNN,3) IMOMENTS APPL AT EACH NODE 


cc 


Quasi-coord (with angular body rates) data 
REAL*8 QSV(NBODES,6+MNM) ! 
REAL*8 QSVD(NBODES,6+MNM) 

REAL*8 QSVDO_1 (NBODES.6+MNM) 
REAL*8 QSVDO_2(NBODES,6+MNM) 


QUASI COORD VECTOR 
IQUASI VECTOR DERIV 
!1 OLD QUASI VECTOR DERIV 
!2 OLD QUASI VECTOR DERIV 


cc Generalized coord (quaternion for orientation) data 

REAL*8 GSV(NBODES,7+MNM) IGEN COORD VECTOR 

REAL*8 GSVD(NBODES,7+MNM) IGEN VECTOR DERIV 

REAL*8 GSVDO_1 (NBODES.7+MNM) !1 OLD GEN VECTOR DERIV 
REAL*8 GSVDO_2(NBODES,7+MNM) ! 2 OLD GEN VECTOR DERIV 


ccccccccccccccccccccccc BODY #1 - TARGET ccccccccccccccccccccc 
cc Position/rate vectors 

REAL*8 P_D10_B1_B1(3) IUNDEFORM D1 POS VECT WRT B1 IN B1 (L) 
REAL*8 P_D1_S1_S1(3) IASSUMED CONST POS D1 WRT SI IN SI (L) 

cc Transformations/quaternions 

REAL*8 T_B1_D1 0(3,3) IUNDEFORMED D1 <-- B1 TRANS 
REAL*8 T_D1_S1(3,3) (ASSUMED CONST TRANS D1 <-S1 

ccccccccccccccccccccccc BODY #2 - TARGET ccccccccccccccccccccc 
cc Position/rate vectors 

REAL*8 P_D20_B2_B2(3) IUNDEFORM D2 POS VECT WRT B2 IN B2 (L) 

cc Transformations/quaternions 

REAL*8 T_B2_D20(3,3) IUNDEFORMED D2 <-- B2 TRANS 


ccccccccccccccccccccccccccc COMMONS ccccccccccccccccccccccccccc 
cc Simulation options 

COMMON / OPTIONS / EOM, INT 

cc Simulation control related commons 
COMMON / SIMCON /TIME, DT 

cc Mass properties related commons 

COMMON / RIGID / MASS, ICM, NNODES, NODES 
COMMON / FLEX / GMASS 

COMMON / MASSINT / GAM_0, GAM_1 , GAM_2JK, M K, l_2JK 
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COMMON / LHS / LU_MASSM, INDX 

cc Modal data related commons 

COMMON / MODES / NMODES, NODE_DOCK, PHI, PSI, 

& MOD_FREQ, MOD_ZETA, PHIETA, PHIETAD, 

& PSIETA, PSIETAD 

cc External body forces/moments per body node commons 
COMMON / NODEFT / FN_BODY, MN_BODY 

cc Coord vector related commons 

COMMON / QSTATE / QSV, QSVD, QSVDOJ , QSVDO_2 
COMMON / GSTATE / GSV, GSVD, GSVDO_1 , GSVDO_2 

cc Body #1 related commons 

COMMON / VECT1 / P_D10_B1_B1 , P_D1_S1_S1 
COMMON /TRANS1 /T_B1_D10, T_D1_S1 

cc Body #2 related commons 

COMMON / VECT2 / P_D20_B2_B2 
COMMON / TRANS2 / T_B2_D20 
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cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

cc 

cc Filename: INTFAC.INC 

cc 

cc Function: Include file for variable/common declarations 

cc of variables necessary to interface to NASA's 

cc code. 

CC 

cc Source: JC Date: 3/91 

cc 

cc Comments: Double precision 

cc 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

REAL*8 T_B1 _l(3,3) [UPDATED IN INTFAC 

REAL*8 T_B2_I(3,3) IUPDATED IN INTFAC 

REAL*8 T_D1_D2(3,3) IUPDATED IN INTFAC 

REAL*8 P_D2_D1_D1(3) IUPDATED IN INTFAC 

REAL*8 PD_D2_D1_D1 (3) UPDATED IN INTFAC 
REAL*8 W_D2_D 1 _D2 (3) IUPDATED IN INTFAC 

REAL*8 T_B1_B2(3,3) IUPDATED IN INTFAC 

REAL*8 T_B2_B1 (3,3) UPDATED IN INTFAC 

REAL*8 P_B2_B1_B1(3) IUPDATED IN INTFAC 

REAL*8 T_D1_B1 (3,3) IUPDATED IN INTFAC 

REAL*8 T_D2_B2(3,3) IUPDATED IN INTFAC 

REAL*8 P_D1_B1_B1 (3) IUPDATED IN INTFAC 

REAL*8 P_D2_B2_B2(3) IUPDATED IN INTFAC 

REAL*8 FS1_S1(3) I FROM BODY #1 F/M SENSOR 

REAL*8 MS1_S1 (3) IFROM BODY #1 F/M SENSOR 

COMMON / BODYJ / T_B1 _l, T_B2 _l 

COMMON / REL_DOCK / T_D1_D2, P_D2_D1_D1 . PD_D2_D1_D1 , 
& W_D2_D1_D2 

COMMON / BODY_BODY / T_B1_B2, T_B2_B1 , P_B2_B1_B1 
COMMON / BODY_DOCK / T_D1_B1 , T_D2_B2, P_D1_B1_B1 , 

& P_D2_B2_B2 

COMMON / FMSENSOR / FS1_S1 , MS1_S1 
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ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
PROGRAM TESTMTBF 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

cc 

Test driver tor checkout of the NAMTB flex 
body simulation; formatted MATLAB output. 


cc Function: 
cc 
cc 

cc Source: 
cc 

cc Comments: 
cc 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
IMPLICIT NONE 


JC Date: 3/91 
Double precision 


cc Include files 

INCLUDE 'BODY.INC' 
INCLUDE 'INTFAC.INC' 


cc Locals 

INTEGER I, NB 

INTEGER NCRT, CRT, NWRIT, WRIT 
INTEGER SEQ(3) 

REAL*8 TSTOP 

REAL*8 TEMP3_1(3), TEMP3_2(3), TEMP3_3(3) 

REAL*8 TEMP3_4(3), TEMP3_5(3), W_B_I_I(3), EA(3) 

REAL*8 TEMP4_1 (4), TEMP4_2(4) 

REAL*8 T_I_B1 (3,3), DCM(3,3), T_I_D1 (3,3), T_B1_D1 (3,3) 

REAL*8 T_I_B2(3,3) 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
cc Initialization 

DATA TIME, NCRT/ 0.0,0/ 

DATA SEQ / 1 , 2, 3 / 

cc First pass sim initialization 
CALL DYNAMIC 

OPEN(UNIT=13,FILE= , MTBSIM.INP , ,FORM= , FORMATTED’,STATUS='OLD') 

OPEN(UNIT=41 ,FILE=’MTB1 .MAT’.FORM^FORMATTED') 

OPEN (U N IT =42 ,FI LE=’MTB2. MAT,FORM='FORMATTED’) 

WRITE(6,*) 

READ(13,*) DT, CRT, WRIT 
NWRIT = WRIT 
NCRT = CRT 
READ(13,*) TSTOP 
READ(13,*) INT 
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CL0SE(13) 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


cc Top of Main Loop 
10 CONTINUE 

cc Ouput to CRT 

IF( NCRT.EQ. CRT) THEN 
NCRT = 0 

WRITE(6 ,*) 'SIM TIME (sec): ’.TIME 
WRITE(6,*) T_B1_I: \(T_B1 J(1 ,l),l=1 ,3) 

WRITE(6,*) T_B1 J: ’,(T_B1 J(2 t l).i=1 ,3) 

WRITE(6,*) T_B1 J: ’,(T_B1 _l(3,l),l=1 ,3) 

WRITE(6,*) 

WRITE(6.*) T_B2_I: '.(T_B2_I(1 ,l),l=1 ,3) 

WRITE(6,*) T_B2J: ',(T_B2J(2,I),I=1 .3) 

WRITE(6,*) T_B2_I: *,(T_B2 _l(3,l),l=1 ,3) 

WRITE(6,*) 

ENDIF 

NCRT = NCRT + 1 

cc Unformatted data output to *.mat files 
IF(NWRIT .EQ. WRIT) THEN 
NWRIT = 0 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
cc PROCESS OUTPUT DATA 
DO 50 1=1 ,3 

TEMP4_1 (I) = GSV(1,3+I) 

TEMP4_2(I) = GSV(2,3+I) 

TEMP3_3(I) = QSV(1 ,3+1) 

TEMP3_4(I) = QSV(2,3+I) 

50 CONTINUE 

TEMP4_1 (4) = GSV(1 ,7) 

TEMP4_2(4) = GSV(2,7) 

CALL Q_T_DCM(TEMP4_1 , DCM) 

CALL DCM_T_EA(DCM, SEQ, EA) 

CALL D_MT R AN S (T_B 1 J , T_I_B1, 3, 3) 

CALL D_M M U L(T_I_B 1 , TEMP3_3, W_B_I_I, 3, 3, 1) 

cc TREETOPS comparable data for BRASS3_2 (two body "contact") run 
CALL D_MTRANS(T_D1 _B1 , T_B1_D1 , 3, 3) 

CALL D_MT R AN S (T_B 1 J , T_I_B1. 3, 3) 

CALL D_M M U L(T_I_B 1 , T_B1_D1 , T_I_D1 , 3, 3, 3) 

CALL D_M M U L(T_I_D 1 , P_D2_D1_D1 , TEMP3_1 , 3, 3, 1) 

CALL D_M M U L(T_I_D 1 , PD_D2_D1_D1 , TEMP3_2, 3, 3, 1 ) 
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NB=1 

WRITE(41 ,444) TIME, (QSV(NB,I),I=1 ,6+NM0DES(NB)), 

& (GSV(NB,I),I=1 ,3), (EA(I),I=1 ,3), (GSV(NB,I), 

& l=8,7+NMODES(NB)), (PHIETA(NB ) 4,I),I=1,3) > 

& (PHIETAD(NB,4,I),I=1 ,3), (PSIETA(NB,4,I),I=1 ,3), 

& (PSIETAD(NB,4,I),I=1 ,3), (TEMP3_1 (I), 1=1 ,3), 

& (TEMP3_2(I),I=1 ,3), (GSVD(NB,I),I=1 ,3), 

& (W_BJJ(I),I=1 ,3) 

cc Body 2 

CALL Q_T_DCM(TEMP4_1 , DCM) 

CALL DCM_T_EA(DCM, SEQ, EA) 

CALL D_MTRANS(T_B2 J , T_I_B2, 3, 3) 

CALL D_M M U L(T_I_B2 , TEMP3_4, W_BJ_I, 3, 3, 1) 

NB = 2 

WRITE(42,444) TIME, (QSV(NB,I),I=1 ,6+NMODES(NB)), 

& (GSV(NB,I),I=1 ,3), (EA(I),I=1 ,3), (GSV(NB,I), 

& l=8,7+NMODES(NB)), (PHIETA(NB,4,I),I=1,3), 

& (PHIETAD(NB,4,I),I=1 ,3), (PSIETA(NB,4,I),I=1 ,3), 

& (PSIETAD(NB,4,I),I=1 ,3), 

& (GSVD(NB,I),I=1 ,3), 

& (W_BJJ(I),I=1,3) 

ENDIF 

NWRIT = NWRIT + 1 
444 FORMAT(2X,50E1 5.7) 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
CALL DYNAMIC 

cc Adjust step size on final step 

IF( (TIME + DT) .GT. TSTOP .AND. (TIME + DT) .LT. 

& (TSTOP + DT/2) ) THEN 
DT = TSTOP -TIME 
NWRIT = WRIT 
NCRT = CRT 
ENDIF 

cc Increment sim time and check for end 
TIME = TIME + DT 
IF(TIME .LE. TSTOP) GOTO 10 

CLOSE(41) 

CLOSE(42) 

cc Create ouput data label files 
open(unit=41 , file='mtb1 .lab’) 
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write(41 ,500) 1 ,'Time ' 
write(41 ,500) 2,'PD_BJ_B ’ 
write(41 ,500) 3,’PD_B_I_B ' 
write(41 ,500) 4,’PD_B_I_B ’ 
write(41 ,500) 5,'W_B_I_B ' 
write(41 ,500) 6,’W_B_I_B ' 
write(41 ,500) 7,’W_B_I_B ' 
write(41 ,500) 8,'ETA_D(1)' 
write(41 ,500) 9,'ETA_D(2) ' 
write(41 ,500) 10,'ETA_D(3) ’ 
write(41 ,500) 1 1 ,'P_B_I J ’ 
write(41 ,500) 12,’P_BJ_I * 
write(41 ,500) 13,’P_B_IJ * 
write(41 ,500) 14,'BJ EA(1)’ 
write(41 ,500) 15,’BJ EA(2)' 
write(41 ,500) 16,'BJ EA(3)’ 
write(41 ,500) 17,’ETA(1) ' 
write(41 ,500) 1 8,'ETA(2) ' 
write(41 ,500) 19,’ETA(3) ’ 
write(41 ,500) 20,'PHIETA4x ’ 
write(41 ,500) 21 ,'PHIETA4y ' 
write(41 ,500) 22,'PHIETA4z ' 
write(41 ,500) 23,'PHIETAD4x’ 
write(41 ,500) 24,'PHIETAD4y’ 
write(41 ,500) 25,’PHIETAD4z' 
write(41 ,500) 26,’PSIETA4x ’ 
write(41 ,500) 27,'PSIETA4y ’ 
write(41 ,500) 28,'PSIETA4z * 
write(41 ,500) 29,'PSIETAD4x' 
write(41 ,500) 30,'PSIETAD4y’ 
write(41 ,500) 31 ,’PSIETAD4z’ 
write(41 ,500) 32,'P_D2_D1_I' 
write(41 ,500) 33,'P_D2_D1 J* 
write(41 ,500) 34,'P_D2_D1_I’ 
write(41 ,500) 35,’PD_D2_D1 i’ 
write(41,500) 36,'PD_D2_D1i’ 
write(41 ,500) 37,'PD_D2_D1 i’ 
write(41 ,500) 38,'PD_B_I_I ' 
write(41 ,500) 39,'PD_B_IJ ' 
write(41 ,500) 40,'PD_B_I_I ' 
write(41 ,500) 41 ,’W_B_I_I * 
write (41 ,500) 42, , W_B_I_I * 
write(41 ,500) 43,’W_B_IJ ' 

open(unit=42, file='mtb2.lab') 

write(42, 500) 1 ,'Time ' 
write(42,500) 2,'PD_B_I_B ' 
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write(42,500) 3,'PD_BJ_B ' 
write(42,500) 4 > ’PD_B_LB ' 
write(42,500) 5,’W_B_I_B ' 
write (42 ,500) 6,'W_BJ_B ’ 
write(42,500) 7,'W_B_I_B ’ 
write(42,500) 8,’ETA_D(1) ’ 
write(42,500) 9,’ETA_D(2) 1 
write(42,500) 10,’ETA_D(3) ’ 
write(42,500) 1 1 /P_BJJ ’ 
write(42,500) 12,'P_B_I_I ' 
write(42,500) 13,'P_BJJ ’ 

write(42,500) 1 4,’B I EA(1)' 

write(42,500) 15,'BJ EA(2)’ 
write(42,500) 16,'BJ EA(3)’ 
write(42,500) 17,'ETA(1) ’ 
write(42,500) 18,'ETA(2) ' 
write(42,500) 19,'ETA(3) * 
write (42 ,500) 20,’PHIETA4x ' 
write(42,500) 21 ,’PHIETA4y ' 
write(42,500) 22,'PHIETA4z ' 
write(42,500) 23, , PHIETAD4x’ 
write(42,500) 24,'PHIETAD4y' 
write(42,500) 25,’PHIETAD4z’ 
write(42,500) 26,'PSIETA4x ' 
write(42,500) 27,'PSIETA4y ' 
write(42,500) 28,’PSIETA4z * 
write(42,500) 29,’PSIETAD4x' 
write(42,500) 30,'PSIETAD4y’ 
write(42,500) 31 , , PSIETAD4z’ 
write(42,500) 32 I , PD_B_I_I ' 
write(42,500) 33,’PD_B_IJ ’ 
write(42,500) 34,’PD_BJJ ' 
write(42,500) 35,'W_BJ_I ’ 
write(42,500) 36,'W_BJ_I ’ 
write (42, 500) 37,'W_B_IJ * 

500 format(4x,i3,2x,a9) 

CLOSE(42) 

CLOSE(42) 

STOP 

END 
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ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

SUBROUTINE DYNAMIC 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

cc 

cc Function: Driver of flexible body contact dynamics model 

cc for the 6-DOF docking/berthing facility. 

CC 

cc Source: JC Date: 2-3/91 

cc 

cc Comments: Double precision 

cc Data via common block declarations 

CC 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

IMPLICIT NONE 


cc Include files 

INCLUDE 'BODY.INC' 


cc Locals 
INTEGER I 

LOGICAL PASS I SUBROUTINE PASS FLAG 

DATA PASS / .FALSE. / 


cc Initialization 

IF(.NOT. PASS) THEN 
PASS = .TRUE. 

CALL IN IT 
CALL INTFAC 
CALL FMTRANS 
CALL CONTROL(I) 
c CALL CONTROLS) 

CALL LUMASSM(I) 

CALL LUMASSM(2) 

CALL PLANT(I) 

CALL PLANT(2) 

RETURN 

ENDIF 

ccccccccccccccccccccc Main Loop cccccccccccccccccccccccccccc 

cc Compute contact forces/torques based on sensor output 
CALL FMTRANS 


cc Compute control system effects 
CALL CONTROL(I) 
c CALL CONTROLS) 

cc Compute time varying mass matrix for Target and Chase if nec 
IF(.NOT. EOM(1 ,1) .OR. .NOT. EOM(1,3)) THEN 
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CALL LUMASSM(I) 

ENDIF 

IF(.NOT. E0M(2,1) .OR. .NOT. EOM(2,3)) THEN 
CALL LUMASSM(2) 

ENDIF 

cc Compute right hand side of EOMs for bodies 1 and 2 
CALL PLANT(I) 

CALL PLANT(2) 

cc Integrate EOMs and handle quasi-to-generalized coord, 
cc transformations 
CALL INTEG(I) 

CALL INTEG(2) 

cc Compute body relative data 
CALL INTFAC 

RETURN 

END 


134.0691.FR 


65 


ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

SUBROUTINE IN IT 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


cc 

cc Function: 

cc 

cc 

cc Source: JC 
cc 

cc Comments: 

cc 

cc 

cc 


Initialization of mass properties, geometry, 
state vectors, etc. 

Date: 2/91 

Double precision 

Data input via reads from ASCII input files 
Data distributed via common block declarations 


ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


IMPLICIT NONE 


cc Include files 

INCLUDE 'BODY.INC' 

INCLUDE 'INTFAC.INC' 

cc Local declarations 
INTEGER I, J, K, L 
INTEGER NB, ROT_SEQ(3) 

REAL*8 PI, RTD 
REAL*8 ROT_ANG(3) 

REAL*8 TEMP1_33(3,3), TEMP2_33(3,3), TEMP3_33(3,3), DCM(3,3) 

REAL*8 Q4(4) 

PARAMETER ( PI = 3.141592654d0 ) 

PARAMETER ( RTD = 180.d0/PI ) 

cc Open pertinent data files 

OPEN(UNIT=1 1 ,FILE=’BODY1 .INP’.FORM^FORMATTED’.STATUS^OLD') 
OPEN(UNIT=12,FILE='BODY2.INP , ,FORM='FORMATTED',STATUS='OLD') 
OPEN(UNIT=20,FILE='INIT.OUT , ,FORM= , FORMATTED , ,STATUS='UNKNOWN') 

cc Read in body #1 (target) data 
NB = 1 


WRITE(20,*) 

WRITE(20,'(1 OX, "BODY #”,12," INITIALIZATION DATA”)') NB 
WRITE(20,*) 

READ(1 1 ,*) MASS(NB) (MASS 

WRITE(20,'(2X,"MASS: M ,F12.4)') MASS(NB) 

READ(1 1 ,*) (ICM(NB,1 ,J), J=1 ,3) IINERTIA MATRIX CM 
READ(1 1 ,*) (ICM(NB,2,J), J=1 ,3) 

READ(1 1 ,*) (ICM(NB,3,J), J=1 ,3) 

WRITE(20,'(2X,"ICM: ",3F1 2.4)’) (ICM(NB,1 ,J),J=1 ,3) 
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WRITE(20 ,'(2X," ICM : ",3F1 2.4)') (ICM(NB,2,J),J=1 ,3) 
WRITE(20,'(2X,"ICM: ".3F12.4)’) (ICM(NB,3,J),J=1,3) 

WRITE(20,*) 

READfl 1 ,*) (GSV(NB,J),J=1 ,3) IBODY CM POS WRT I IN I 
READ(1 1 ,*) (QSV(NB,J),J=1 ,3) IBODY TRANS RATE WRT I IN BODY 
READ(1 1 ,*) (QSV(NB,J),J=4,6) IBODY ANG RATE WRT I IN BODY 

READfl 1 ,*) (ROT_SEQ(J), J=1 ,3) IBODY <-- 1 ROT SEQUENCE 
READ(1 1 ,*) (ROT_ANG(J), J=1 ,3) IBODY <-- 1 ROT ANGLES (DEG) 

cc Compute trans from Inertial (I) to Body 1 (target) from 
cc Euler angle sequence (I <-- Body) 

DO 3 J=1 ,3 

QSV(NB,3+J) = QSV(NB,3+J) / RTD 
ROT_ANG(J) = ROT_ANG(J) / RTD 
3 CONTINUE 

CALL D_ROT(ROT_SEQ(1 ), ROT_ANG(1), TEMP1_33) 

CALL D_ROT ( ROT_S EQ(2) , ROT_ANG(2), TEMP2_33) 

CALL D_ROT(ROT_SEQ(3) , ROT_ANG(3), TEMP3_33) 

CALL D_MMUL(TEMP1_33, TEMP2_33, DCM, 3, 3, 3) 

CALL D_MMUL(DCM, TEMP3_33, TEMP1_33, 3, 3, 3) 

CALL D_MTRANS(TEMP1_33, T_B1_I, 3, 3) 

cc Compute BODY <-- 1 quaternion and put in generalized state vect 
CALL DCM_T_Q(T_B1 J, Q4) 

CALL D_QNORM(Q4) 

WRITE(20,*) ' T_B1 J: ',(T_B1 _l(1 ,l),l-1,3) 

WRITE(20,*) ’ T B 1 I : *,(T_B1 _l(2,l),l=1 ,3) 

WRITE(20,*) ' T_B1 J: \(T_B1 _l(3,l),l=1 ,3) 

WRITE(20,*) 

DO 5 J=1 ,4 
GSV(NB,3+J) = Q4(J) 

5 CONTINUE 

WRITE(20,'(3X,"ANG RATES CONVERTED TO (RAD/S) IN SVs")') 
WRITE(20, , (2X,"QSV(1 ->6): ",6F1 2.5)') (QSV(NB,J),J=1 ,6) 
WRITE(20,*) 

WRITE(20,'(2X,"GSV(1 ->7): ",7F1 2.5)’) (GSV(NB,J),J=1 ,7) 
WRITE(20,*) 

cc Body node data 

READ(1 1 ,*) NNODES(NB), NODE_DOCK(NB) 
WRITE(20/(2X,"NNODES: 'MS.SX/'NODE.DOCK: ",13)’) 

& NNODES(NB), NODE_DOCK(NB) 
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DO 10 1=1 ,NNODES(NB) 

READ(1 1 ,*) (NODES(NB,l,J),J=1 ,3) 'BODY NB NODES 
WRITE(20, , (2X,"NODE ",12,”: ".10F12.5)') 

& I, (NODES(NB,l,J),J=1 ,3) 

10 CONTINUE 
WRITE(20,*) 

cc Sensor/docking port location/orientation initialization 
DO 15 1=1 ,3 

P_D10_B1_B1 (I) = NODES(NB,NODE_DOCK(NB),l) 

15 CONTINUE 

READ(1 1 ,*) (P_D1_S1_S1 (l),l=1 ,3) 

READ(1 1 ,*) (T_B1_D1 0(1 ,l),l=1 ,3) 

READ(11 ,*) (T_B1_D1 0(2,1), 1=1 ,3) 

READ(11 ,*) (T_B1_D1 0(3,1), 1=1 ,3) 

READ(1 1 ,*) (T_D1_S1 (1 ,l),l=1 ,3) 

READ(1 1 ,*) (T_D1_S1 (2,1), 1=1 ,3) 

READ(1 1 ,*) (T_D1_S1 (3,1), 1=1 ,3) 

cc Equations of motion complexity options 

READ(1 1 ,*) (EOM(NB,l),l=1 ,3) ILOGICAL EOM OPTIONS 
WRITE(20,'(2X,"EOM FLAGS (EHOMIT; ERNLT; FF MODES): ",3L6)') 
& (EOM(NB,l),l=1,3) 

cc EOM options: ERNLT implies EHOMIT 
IF( EOM(NB,2) ) EOM(NB,1) = .TRUE. 

cc Flex data 

READ(1 1 ,*) NMODES(NB) 

WRITE(20,'(2X,"NMODES: ”,13)') NMODES(NB) 

IF(NMODES(NB) .EQ. 0) GOTO 200 

cc Modal frequencies (rad/sec) 

READ(1 1 ,*) (MOD_FREQ(NB,l),l=1 .NMODES(NB)) 
WRITE(20,'(2X,"MOD_FREQ: M0F12.4)') (MOD_FREQ(NB,l), 

& l=1,NMODES(NB)) 

cc Modal damping 

READ(1 1 ,*) (MOD_ZETA(NB,l),l=1 .NMODES(NB)) 
WRITE(20,'(2X,"MOD_ZETA: ,, ,10F12.4)') (MOD_ZETA(NB,l), 

& l=1,NMODES(NB)) 

cc Init modal coords (eta) and modal coord derivs (etad) 

READ(1 1 ,*) (GSV(NB,7+I),I=1 ,NMODES(NB)) 

READ(1 1 ,*) (QSV(NB,6+I),I=1 .NMODES(NB)) 
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WRITE(20,’(2X,"ETA: ",1 0F1 2.4)’) (GSV(NB,7+I),I=1 .NMODES(NB)) 
WRITE(20,'(2X,"ETAD: ",1 0F1 2.4)') (QSV(NB,6+I),I=1 ,NMODES(NB)) 
WRITE(20,*) 

cc Generalized mass matrix 
DO 25 1=1 .NMODES(NB) 

READ(1 1 ,*) (GMASS(NB,I,J),J=1 ,NMODES(NB)) IGEN MASS MAT 
25 CONTINUE 

cc Translation modal gains 
DO 35 J=1 .NNODES(NB) 

DO 30 K=1 .NMODES(NB) 

READ(1 1 ,*) (PHI(NB,J,K,I),I=1 ,3) 

30 CONTINUE 
35 CONTINUE 

cc Rotation modal gains 
DO 45 J=1 .NNODES(NB) 

DO 40 K=1 .NMODES(NB) 

READ(1 1 ,*) (PSI(NB,J,K,I) ,1=1 ,3) 

40 CONTINUE 
45 CONTINUE 

cc Standard mass integral terms 
DO 50 J=1 .NMODES(NB) 

READ(1 1 ,*) (GAM_0(NB,l,J), 1=1 ,3) !GAM_0 MASS INTEGS 
50 CONTINUE 

DO 60 J=1 .NMODES(NB) 

READ(11,*) (GAM_1(NB,I,J), 1=1,3) !GAM_1 MASS INTEGS 
60 CONTINUE 

cc Zero standard MITs for free-free mode option 
IF( EOM(NB,3) ) THEN 
DO 65 J=1 ,NMODES(NB) 

DO 66 1=1 ,3 
GAM_0(NB,l,J) = O.dO 
GAM_1 (NB.I.J) = O.dO 
66 CONTINUE 
65 CONTINUE 
ENDIF 

cc Higher order mass integral terms 
DO 70 1=1 ,NMODES(NB) 

DO 80 J=1 .NMODES(NB) 

READ(1 1,*) (GAM_2JK(NB,I,J,K),K=1 ,3) !GAM_2JK MASS INTEGS 
80 CONTINUE 
70 CONTINUE 
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DO 90 J=1 .NMODES(NB) 

DO 100 K=1 ,3 

READ(1 1 ,*) (l_1 K(NB,J,K,L),L=1 ,3) !l_1 K MASS INTEGS 

100 CONTINUE 
90 CONTINUE 

DO 110 1=1 ,NMODES(NB) 

DO 120 J=1 ,NMODES(NB) 

DO 130 K=1 ,3 

READ(1 1 ,*) (l_2JK(NB,l,J,K,L),L=1 ,3) !M K MASS INTEGS 
130 CONTINUE 
120 CONTINUE 
110 CONTINUE 

200 CONTINUE 

cc Read in body #2 (CHASE) data 
NB = 2 

WRITE(20,*) 

WRITE(20,*) 

WRITE(20,'(1 0X,"BODY #",12,” INITIALIZATION DATA")') NB 
WRITE(20,*) 

READ(12,*) MASS(NB) IMASS 

WRITE(20,'(2X,"MASS: ",F12.4)') MASS(NB) 

READ(1 2,*) (ICM(NB,1 ,J), J=1 ,3) ilNERTIA MATRIX CM 
READ(12,*) (ICM(NB,2,J), J=1 ,3) 

READ(12,*) (ICM(NB,3,J), J=1,3) 

WRITE(20,'(2X,"ICM: ",3F1 2.4)’) (ICM(NB,1 ,J),J=1 ,3) 
WRITE(20,'(2X,"ICM: ",3F1 2.4)') (ICM(NB,2,J),J=1 ,3) 
WRITE(20,'(2X,"ICM: ",3F1 2.4)') (ICM(NB,3,J),J=1 ,3) 

WRITE(20,*) 

READ(1 2,*) (GSV(NB,J),J=1 ,3) IBODY CM POS WRT I IN I 
READ(12,*) (QSV(NB,J),J=1 ,3) IBODY TRANS RATE WRT I IN BODY 
READ(1 2,*) (QSV(NB,J),J=4,6) IBODY ANG RATE WRT I IN BODY 
READ(1 2,*) (ROT_SEQ(J), J=1 ,3) IBODY <- I ROT SEQUENCE 
READ(1 2,*) (ROT_ANG(J), J=1 ,3) IBODY <- I ROT ANGLES (DEG) 


cc Compute trans from Inertial (I) to Body 2 (CHASE) from 
cc Euler angle sequence (I <-- Body) 

DO 203 J=1 ,3 

QSV(NB,3+J) = QSV(NB,3+J) / RTD 
ROT_ANG(J) = ROT_ANG(J) / RTD 
203 CONTINUE 

CALL D_ROT(ROT_SEQ(1 ), ROT_ANG(1), TEMP1_33) 
CALL D_ROT(ROT_SEQ(2), ROT_ANG(2), TEMP2_33) 
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CALL D_ROT ( ROT_S EQ(3) , R0T_ANG(3), TEMP3_33) 

CALL D_M MU L(TE MP 1 _33 , TEMP2_33, DCM, 3, 3, 3) 

CALL D_MMUL(DCM, TEMP3_33, TEMPI _33, 3, 3, 3) 

CALL D_MTRANS(TEMP1_33, T_B2_I, 3, 3) 

cc Compute BODY <-- 1 quaternion and put in generalized state vect 
CALL DC M_T_Q (T_B2_I , Q4) 

CALL D_QNORM(Q4) 

WRITE(20,*) ' T_B2J: , ,(T_B2_I(1 ,l),l=1 ,3) 

WRITE(20,*) ’ T_B2J: \(T_B2J(2,I),I=1 ,3) 

WRITE(20,*) ’ T B2_l : \(T_B2_I(3,I),I=1 ,3) 

WRITE(20,*) 

DO 205 J=1 ,4 
GSV(NB,3+J) = Q4(J) 

205 CONTINUE 

WRITE(20,'(3X ) "ANG RATES CONVERTED TO (RAD/S) IN SVs")’) 
WRITE(20, , (2X,"QSV(1 ->6): ’’.eF^^)’) (QSV(NB,J),J=1,6) 

WRITE (20,*) 

WRITE(20,'(2X,"GSV(1 ->7): ".7F12.5)') (GSV(NB,J),J=1,7) 
WRITE(20,*) 

cc Body node data 

READ(12,*) NNODES(NB), NODE_DOCK(NB) 
WRITE(20, , (2X,"NNODES: ".IS.SX/'NODEJDOCK: ",I3)’) 

& NNODES(NB), NODE_DOCK(NB) 

DO 210 1=1 .NNODES(NB) 

READ(1 2,*) (NODES(NB,l,J),J=1 ,3) IBODY NB NODES 
WRITE(20,'(2X,"NODE ",I2,": ",10F12.5)') 

& I, (NODES(NB,l,J),J=1,3) 

210 CONTINUE 
WRITE(20,*) 

cc Sensor/docking port location/orientation initialization 
DO 21 5 1=1 ,3 

P_D20_B2_B2(I) = NODES(NB,NODE_DOCK(NB),l) 

215 CONTINUE 

READ(12,*) (T_B2_D20(1 ,l),l=1 ,3) 

READ(12,*) (T_B2_D20(2,I),I=1 ,3) 

READ(12,*) (T_B2_D20(3,I),I=1 ,3) 

cc Equations of motion complexity options 

READ(12,*) (EOM(NB,l),l=1 ,3) ILOGICAL EOM OPTIONS 
WRITE(20,’(2X,"EOM FLAGS (EHOMIT; ERNLT; FF MODES): ",3L6)') 
& (EOM(NB,l),l=1 ,3) 
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cc EOM options: ERNLT implies EHOMIT 
IF( EOM(NB,2) ) EOM(NB.I) = .TRUE. 


cc Flex data 

READ(12,*) NMODES(NB) 

WRITE(20,'(2X,"NMODES: ",I3)’) NMODES(NB) 

IF(NMODES(NB) .EQ. 0) GOTO 400 

cc Modal frequencies (rad/sec) 

READ(1 2,*) (MOD_FREQ(NB,l),l=1 .NMODES(NB)) 
WRITE(20 ) '(2X,"MOD_FREQ: ",10F12.4)’) (MOD_FREQ(NB,l), 

& 1=1 .NMODES(NB)) 

cc Modal damping 

READ(1 2,*) (MOD_ZETA(NB,l),l=1 ,NMODES(NB)) 
WRITE(20,'(2X,"MOD_ZETA: ".10F12.4)') (MOD_ZETA(NB,l), 

& 1=1 .NMODES(NB)) 

cc Init modal coords (eta) and modal coord derivs (etad) 

READ(12,*) (GSV(NB,7+I),I=1 .NMODES(NB)) 

READ(12,*) (QSV(NB,6+I),I=1 .NMODES(NB)) 

WRITE(20,'(2X,"ETA: ",1 0F1 2.4)') (GSV(NB,7+I),I=1 .NMODES(NB)) 
WRITE(20,'(2X,"ETAD: ",1 0F1 2.4)') (QSV(NB,6+I),I=1 .NMODES(NB)) 
WRITE(20,*) 

cc Generalized mass matrix 
DO 245 1=1 .NMODES(NB) 

READ(12,*) (GMASS(NB,I,J),J=1 ,NMODES(NB)) IGEN MASS MAT 
245 CONTINUE 

cc Translation modal gains 
DO 225 J=1 .NNODES(NB) 

DO 230 K=1 .NMODES(NB) 

READ(12,*) (PHI(NB,J,K,I),I=1 ,3) 

230 CONTINUE 
225 CONTINUE 

cc Rotation modal gains 
DO 235 J=1 .NNODES(NB) 

DO 240 K=1 .NMODES(NB) 

READ(12 f *) (PSI(NB,J,K,I),I=1 ,3) 

240 CONTINUE 
235 CONTINUE 

cc Standard mass integral terms 
DO 250 J=1 .NMODES(NB) 

READ(12,*) (GAM_0(NB,l,J), 1=1,3) !GAM_0 MASS INTEGS 
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250 CONTINUE 


DO 260 J=1 .NMODES(NB) 

READ(12,*) (GAM_1(NB,I,J), 1=1,3) !GAM_1 MASS INTEGS 
260 CONTINUE 

cc Zero standard MITs for free-free mode option 
IF( EOM(NB,3) ) THEN 
DO 265 J=1 .NMODES(NB) 

DO 266 1=1 ,3 
GAM_0(NB,l,J) = O.dO 
GAM_1 (NB.I.J) = O.dO 
266 CONTINUE 
265 CONTINUE 
ENDIF 

cc Higher order mass integral terms 
DO 270 1=1 .NMODES(NB) 

DO 280 J=1 .NMODES(NB) 

READ(12,*) (GAM_2JK(NB,I,J,K),K=1 ,3) !GAM_2JK MASS INTEGS 
280 CONTINUE 
270 CONTINUE 

DO 290 J=1 .NMODES(NB) 

DO 300 K=1 ,3 

READ(12,*) (l_1 K(NB,J,K,L),L=1 ,3) !I_1K MASS INTEGS 

300 CONTINUE 
290 CONTINUE 

DO 310 1=1 .NMODES(NB) 

DO 320 J=1 ,NMODES(NB) 

DO 330 K=1 ,3 

READ(1 2,*) (l_2JK(NB,l,J,K,L),L=1 ,3) !l_1 K MASS INTEGS 
330 CONTINUE 
320 CONTINUE 
310 CONTINUE 

400 CONTINUE 

CLOSE(II) 

CLOSE(12) 

CLOSE(20) 

RETURN 

END 
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ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

SUBROUTINE FMTRANS 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


cc 

cc Function: 


cc 

cc 

cc 

cc Source: 
cc 

cc Comments: 

cc 

cc 


Computes contact forces/torques at docking port 
nodes in respective body frames based on output 
of body #1 (target) force/moment sensor. 

JC Date: 3/91 

Double precision 

Data via common block declarations 


ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


IMPLICIT NONE 


cc Include files 

INCLUDE 'BODY.INC' 
INCLUDE 'INTFAC.INC' 


cc Locals 

INTEGER I, J, K 

REAL*8 TEMPI _3(3), TEMP2_3(3) 

REAL*8 FS1_B1(3), FS1_B2(3) 

REAL*8 MD1_S1(3), MD2_S1(3), MD1_B1(3), MD2_B2(3) 
REAL*8 T_B1_S1 (3,3), T_B2_S1 (3,3) 

REAL*8 P_D2_S1_S1 (3) 

REAL*8 F_D1 (3), M_D1 (3), T_S1_D1 (3,3) 

ccccccccc "Contact" Test Case cccccccccccccccccccccccccc 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

cc Compute sensed F/M based on comparable TREETOPS inputs 
IF(TIME.LT. 0.0199) THEN 
F_D1 (1 ) = -3000.d0 
F_D1 (2) = 2000. DO 
F_D1 (3) = 0.D0 
M_D1 (1 ) = 0.D0 
M_D1 (2) = 15000. DO 
M_D1 (3) = 0.D0 

C FN_BODY(1 ,4,1 ) = -3000. DO 

c FN_BODY(1 ,4,2) = 2000. DO 

c FN_BODY(1 ,4,3) = 0.D0 

c MN_BODY(1 ,4,1) = 0.D0 

C MN_BODY(1 ,4,2) = 1 5000.D0 

c MN_BODY(1 ,4,3) = 0.D0 

CALL D_MTRANS(T_D1_S1, T_S1_D1, 3, 3) 

CALL D_MXV (T_S1 _D1 , F_D1 , FS1_S1 , 3, 3) 
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CALL D_MXV(T_S1_D1 , M_D1 , MS1_S1 , 3, 3) 

CALL D_VCR0SS(P_D1_S1_S1 , FS1_S1, TEMP1_3) 

DO 1=1 ,3 

MS1_S1(I) = MS1_S1(I) + TEMP1_3(I) 

ENDDO 

ELSE 
DO 1=1 ,3 

FS1 SI (I) = 0.D0 

MS1_S1 (I) = 0.D0 
ENDDO 
ENDIF 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


cc Transform sensed forces to respective body fixed sytems 
cc Assume rigid connection from sensor to docking port 
cc (i.e., T_D1_S1 remains constant) 

DO 4 1=1 ,3 
DO 5 K=1 ,3 
T_B1_S1 (l,K) = 0.D0 

5 CONTINUE 
DO 6 J=1 ,3 

T_B1_S1 (l,J) = T_B1_S1 (l,J) + T_D1_B1 (J,l) * T_D1_S1 (J,l) 

6 CONTINUE 
4 CONTINUE 

CALL D_M M U L(T_B 2_B 1 , T_B1_S1 , T_B2_S1 , 3, 3, 3) 

CALL D_MXV (T_B 1 _S 1 , FS1_S1 , FS1_B1 , 3, 3) 

CALL D_MXV (T_B2_S 1 , FS1_S1 , FS1_B2, 3, 3) 

cc Equal and opposite contact forces; all contact points sensed 
cc by force/moment sensor on body #1 
DO 10 1=1,3 

FN_BODY(1,NODE_DOCK(1),l) = FS1_B1(I) 
FN_BODY(2,NODE_DOCK(2),l) = -FS1_B2(I) 

10 CONTINUE 

cc Compute vector from SI to D1 and D2 
DO 15 1=1 ,3 

TEMP1_3(I) = 0.D0 
DO 16 J=1 ,3 

TEMP1_3(I) = TEMP1_3(I) + T_D1_S1(J,I) * P_D2_D1_D1 (J) 
16 CONTINUE 
15 CONTINUE 

DO 20 1=1 ,3 

P_D2_S1_S1(I) = P_D1_S1_S1(I) + TEMP1_3(I) 

20 CONTINUE 
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cc Compute contact moments (at D1 and D2) in respect body coords 
CALL D_VCROSS(P_D1_S1_S1 , FS1_S1, TEMP1_3) 

CALL D_VCROSS(P_D2_S1_S1 , FS1_S1 , TEMP2_3) 

DO 40 1=1 ,3 

MD1_S1 (I) = MS1_S1 (I) - TEMPI _3(l) 

MD2_S1 (I) = -MS1_S1 (I) + TEMP2_3(I) 

40 CONTINUE 

CALL D_MXV(T_B1_S1 . MD1_S1 , MD1_B1 . 3, 3) 

CALL D_MXV (T_B2_S 1 , MD2_S1 , MD2_B2, 3, 3) 

cc Add contact moments to node moment vector 
DO 50 1=1 ,3 

MN_BODY(1 ,NODE_DOCK(1),l) = MD1_B1(I) 
MN_BODY(2,NODE_DOCK(2),l) = MD2_B2(I) 

50 CONTINUE 

RETURN 

END 
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ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

SUBROUTINE CONTROL(NB) 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


cc 

cc Function: 

cc 

cc 

cc Source: 
cc 

cc Comments: 

cc 

cc 

cc 

cc 


Computes control forces and torques applied to 
bodies in the respective body fixed frames. 

JC Date: 3/91 

Double precision 

Data via common block declarations 

File serves as a place holder when no control 

is implemented 


ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


cc Include files 

INCLUDE 'BODY.INC' 
INCLUDE 'INTFAC.INC' 


cc Locals 


RETURN 

END 
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ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

SUBROUTINE LUMASSM(NB) 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


cc 

cc Function: 

cc 

cc 

cc 

cc 

cc Source: 
cc 

cc Comments: 

cc 

cc 


Computes mass matrix of body #NB in lower-upper 
decomposed format (i.e., LU decomposition); 
the resulting system can be solved using 
forwards/backwards substitution. 

JC Date: 2/91 

Double precision 

Data via common block declarations 


ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

IMPLICIT NONE 


cc Include files 

INCLUDE 'BODY. INC' 


cc Local declarations 
INTEGER I, J, K, L 
INTEGER NB, NM, INDXX(6+MNM) 

REAL*8 MASSM(6+MNM,6+MNM) 

REAL*8 TEMP_3MNM(3,MNM), TEMP_MNM(MNM) 

REAL*8 GAM0_ETA(3), G0_ETA_T(3,3), GAM_2(3,MNM) 
REAL*8 II 1 (3,3), ll_2(3,3), D 

cc Start building lower triangular mass matrix (symmetric) 
cc Add diagonal mass submatrix (3x3) assoc with rigid body trans 
MASSM(I.I) = MASS(NB) 

MASSM(2,2) = MASS(NB) 

MASSM(3,3) = MASS(NB) 

cc Add rigid body moment of inertia about body CM submatrix (3x3) 
cc to lower triangular mass matrix 
DO 10 1=1 ,3 
DO 20 J=1 ,1 

MASSM(3+I,3+J) = ICM(NB,I,J) 

20 CONTINUE 
10 CONTINUE 

cc Check for no flex body modes 
NM = NMODES(NB) 

IF(NM .EQ. 0) GOTO 500 

cc Add generalized mass submatrix (NMxNM) assoc with modal 
cc coords to lower triangular mass matrix 
DO 30 1=1 ,NM 
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DO 40 J=1,l 

MASSM(6+I,6+J) = GMASS(NB,I,J) 

40 CONTINUE 
30 CONTINUE 

cc Coupling between rigid body translation and rotation 
cc via mass integral terms 

cc Pull gammaO (3xNM) matrix and eta vector (NMxl) out 
IF( .NOT. EOM(NB,3) ) THEN 

CALL D_ZERO(TEMP_3MNM,3,MNM) 

DO 50 J=1 ,NM 
DO 60 1=1 ,3 

TEMP_3MNM(I,J) = GAM_0(NB,l,J) 

60 CONTINUE 

TEMP_MNM(J) = GSV(NB,7+J) 

50 CONTINUE 

cc Create gamma_0 (3 x MNM) * eta (MNM x 1) tilde matrix 

CALL D_MMUL(TEMP_3MNM,TEMP_MNM,GAM0_ETA,3,MNM,1 ) 
CALL D_TILDE(GAM0_ETA, G0_ETA_T) 

ELSE 

CALL D_ZE RO (G 0_ET A_T , 3, 3) 

ENDIF 

cc Add gam0_eta tilde (3x3), gamO (3xNM), and garni (3xNM) 
cc submatrices to lower triangular mass matrix 
DO 70 1=1 ,3 
DO 80 J=1 ,3 

MASSM(3+I,J) = G0_ETA_T(l,J) 

80 CONTINUE 
DO 90 J=1,NM 

MASSM(6+J,I) = GAM_0(NB,l,J) 

MASSM(6+J,3+I) = GAM_1 (NB,I,J) 

90 CONTINUE 
70 CONTINUE 

cc Check for elimination of higher order mass integral terms 
IF(EOM(NB,1)) GOTO 500 

cc Compute gamma2 (3 x NM) and add to lower triangular mass mat 
DO 100 1=1 ,3 
DO 110 J=1 ,NM 
GAM_2(I,J) = 0.0D0 
DO 120 K=1 ,NM 

GAM_2(I,J) = GAM_2(I,J) + GSV(NB,7+K)* 

& GAM_2JK(NB,K,J,I) 

120 CONTINUE 

MASSM(6+J,3+I) = MASSM(6+J,3+I) + GAM_2(I,J) 
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110 CONTINUE 
100 CONTINUE 


cc Compute ll_1 (3 x 3) and ll_2 (3 x 3); HOM-ITs affecting 
cc moment of inertia matrix 
DO 170 1=1,3 
DO 180 J=1 ,3 

II 1 (l,J) = 0.0D0 

ll_2(l,J) = 0.0D0 
DO 190 K=1 ,NM 

ll_1 (l,J) = ll_1 (l,J) + GSV(NB,7+K)* 

& ( l_1 K(NB,K,I,J) + l_1 K(NB,K,J,I) ) 

DO 200 L=1 ,NM 

ll_2(l,J) = ll_2(l,J) + 0.5*GSV(NB,7+K)*GSV(NB,7+L)* 

& ( l_2JK(NB,K,L,l,J) + l_2JK(NB,L,K,l,J) ) 

200 CONTINUE 
190 CONTINUE 
180 CONTINUE 
170 CONTINUE 

cc Add II 1 and ll_2 to lower triangular mass matrix 

DO 220 1=1 ,3 
DO 230 J=1 ,1 

MASSM(3+I,3+J) = MASSM(3+I,3+J) + ll_1(l,J) + ll_2(l,J) 

230 CONTINUE 
220 CONTINUE 

cc Now have FUN-WHOM-IT mass matrix 
500 CONTINUE 

cc Fill in upper triangle of mass matrix using symmetry prop 
DO 400 1=1 ,6+NM 
DO 420 J=I,6+NM 
MASSM(I.J) = MASSM(J.I) 

420 CONTINUE 
400 CONTINUE 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
c WRITE(6,*) 
c WRITE(6,*) ' MASSM:' 
c DO 425 1=1 ,6+NM 

c WRITE(6,'(1 X,1 0E1 2.5)') (MASSM(I,J),J=1 ,6+NM) 
c425 CONTINUE 
c WRITE(6,*) 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 

cc Compute lower-upper decomposition of mass matrix 
CALL LUDCMP(MASSM,6+NM,6+MNM,INDXX,D) 
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cc Record in memory 
DO 510 1=1 ,6+NM 
INDX(NB.I) = INDXX(I) 

DO 520 J=1 ,6+NM 
LU_MASSM(NB,I,J) = MASSM(I.J) 
520 CONTINUE 
510 CONTINUE 

RETURN 

END 
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ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

SUBROUTINE PLANT(NB) 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


cc 

cc Function: 


cc 

cc 

cc 

cc Source: 
cc 

cc Comments: 

cc 

cc 


Computes right hand side of equations of motion. 
These terms represent the effects of damping, 
stiffness, and forcing functions. 

JC Date: 2/91 


Double precision 

Data via common block declarations 


ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
IMPLICIT NONE 


cc Include files 

INCLUDE ’BODY.INC' 

cc Local declarations 
INTEGER I, J, K, L, M, N 
INTEGER NB, NM, INDXX(6+MNM) 

REAL*8 MASSM(6+MNM,6+MNM) 

REAL*8 TEMP1_MNM3(MNM,3), TEMP2_MNM3(MNM,3) 
REAL*8 TEMP1_3MNM(3,MNM), TEMP2_3MNM(3,MNM) 
REAL*8 TEMP1_3(3), TEMP2_3(3), TEMP3_3(3), TEMP4_3(3) 
REAL*8 TEMP5_3(3), TEMP6_3(3), TEMP_33(3,3) 

REAL*8 TEMP1_MNM(MNM), TEMP2_MNM(MNM) 

REAL*8 GAM0_ETA(3), GAM0_ETAD(3) 

REAL*8 GAM_2D(3,MNM), GAM_2(3,MNM) 

REAL*8 RHS(6+MNM), FNM(MNM), TNM(MNM) 

REAL*8 F_BODY(3), T_BODY(3) 

REAL*8 WB(3) 

REAL*8 IM (3,3), ll_2(3,3), ll_1D(3,3), ll_2D(3,3) 

REAL*8 11(3,3), IID(3,3) 

REAL*8 D_DOT 

cc Rigid body translational/rotational forcing functions 
cc Based on nodes locations defined over each body 
cc Mult forces, pure torques, and POS x FORCE torques due to 
cc node forces are summed into a single force/torque at CM. 
CALL D_ZERO(F_BODY, 3, 1) 

CALL D_ZERO(T_BODY, 3, 1) 

CALL D_ZERO(RHS, 6+MNM, 1) 

NM = NMODES(NB) 

cc Compute torque at CM due to POS x FORCE of node forces 
cc Include flexibility effects on node position vectors 
DO 10 N=1 .NNODES(NB) 
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DO 15 J=1,3 

TEMP1_3(J) = NODES(NB,N,J) + PHIETA(NB,N,J) 

TEMP2_3(J) = FN_BODY(NB,N,J) 

15 CONTINUE 

CALL D_VCROSS(TEMP1_3, TEMP2_3, TEMP3_3) 

DO 20 J=1 ,3 

F_BODY(J) = F_BODY(J) + FN_BODY(NB,N,J) 

T_BODY(J) = T_BODY(J) + MN_BODY(NB,N,J) +TEMP3_3(J) 

20 CONTINUE 
10 CONTINUE 

cc Build right hand side terms; pull out body ang vel vector 
DO 30 J=1 ,3 
RHS(J) = F_BODY(J) 

RHS(3+J) = T_BODY(J) 

WB(J) = QSV(NB,3+J) 

30 CONTINUE 

cc Check for no flex body modes 
IF(NM .EQ. 0) GOTO 500 

cc Compute EHOMIT + ERNLT flex RHS terms 
cc Generalized flex eqns forcing functions 
DO 40 N=1 .NNODES(NB) 

DO 50 M=1 ,NM 
DO 60 1=1 ,3 

TEMP1_MNM3(M,I) = PHI(NB,N,M,I) 

TEMP2_MNM3(M,I) = PSI(NB,N,M,I) 

60 CONTINUE 
50 CONTINUE 
DO 70 1=1 ,3 

TEMPI _3(l) = FN_BODY(NB,N,l) 

TEMP2_3(I) = MN_BODY(NB,N,l) 

70 CONTINUE 

CALL D_MMUL(TEMP1_MNM3, TEMP1_3, FNM, MNM, 3, 1) 

CALL D_MMUL(TEMP2_MNM3, TEMP2_3, TNM, MNM, 3, 1) 

DO 80 M=1 ,NM 

RHS(6+M) = RHS(6+M) + FNM(M) + TNM(M) 

80 CONTINUE 
40 CONTINUE 

cc Strain energy and damping terms 
DO 90 M=1 ,NM 

RHS(6+M) = RHS(6+M) - GSV(NB,7+M) * MOD_FREQ(NB,M)**2 - 
& 2.D0 * MOD_ZETA(NB,M) * MOD_FREQ(NB,M) * GSVD(NB,7+M) 

90 CONTINUE 

cc Check for EHOMIT + ERNLT option; ERNLT encompasses EHOMIT 
IF(EOM(NB,2)) GOTO 500 
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cc Nonlinear translational RHS terms 

cc Pull out gammaO (3xNM) matrix, gamml (3xNM), 

cc eta vector (NMxl ), and etad vector (NMxl ) 

CALL D_ZERO(TEMP1_3MNM,3,MNM) 

CALL D_ZERO(TEMP2_3MNM,3,MNM) 

DO 100 J=1 ,NM 
DO 110 1=1,3 

TEMP1_3MNM(I,J) = GAM_0(NB,l,J) 

TEMP2_3MNM(I,J) = GAM_1(NB,I,J) 

110 CONTINUE 

TEMP1_MNM(J) = GSV(NB,7+J) 

TEMP2_MNM(J) = QSV(NB,6+J) 

100 CONTINUE 

cc Create gamma_0 (3 x MNM) * eta (MNM x 1) vector 
cc Compute wb x (wb x gam0*eta) 

IF( .NOT. EOM(NB,3) ) THEN 

CALL D_MMUL(TEMP1_3MNM, TEMP1_MNM, GAM0_ETA, 3, MNM, 1) 
CALL D_VCROSS(WB, GAM0_ETA, TEMP1_3) 

CALL D_VCROSS(WB, TEMPI _3, TEMP2_3) 

ELSE 

TEMP2_3(1) = 0.D0 
TEMP2_3(2) = 0.D0 
TEMP2_3(3) = 0.D0 
ENDIF 

cc Compute wb x (mass*Rdcmb + 2*gam0*etad) 

IF( .NOT. EOM(NB,3) ) THEN 

CALL D_MMUL(TEMP1_3MNM, TEMP2_MNM, GAM0_ETAD, 3, MNM, 1) 
ELSE 

GAM0_ETAD(1) = 0.D0 
GAM0_ETAD(2) = 0.D0 
GAM0_ETAD(3) = 0.D0 
ENDIF 

DO 120 1=1 ,3 

TEMP1_3(I) = MASS(NB) * QSV(NB,I) + 

& 2.D0 * GAM0_ETAD(l) 

120 CONTINUE 

CALL D_VCROSS(WB, TEMP1_3, TEMP3_3) 


cc Add translational terms to RHS 
DO 130 1=1,3 

RHS(I) = RHS(I) - TEMP2_3(I) - TEMP3_3(I) 
TEMP1_3(I) = QSV(NB,I) 

130 CONTINUE 
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cc Nonlinear rotational RHS terms 
cc wb x (garni * etad) 

IF( .NOT. EOM(NB,3) ) THEN 

CALL D_MMUL(TEMP2_3MNM, TEMP2_MNM, TEMP2_3, 3, MNM, 1) 
CALL D_VCROSS(WB, TEMP2_3, TEMP3_3) 


cc gamO*eta x ( wb x Rdcmb) 

CALL D_VCROSS(WB, TEMPI _3, TEMP4_3) 

CALL D_VCROSS(GAMO_ETA, TEMP4J3, TEMP5_3) 

ELSE 

DO 135 1=1,3 
TEMP3_3(I) = 0.D0 
TEMP5_3(I) = 0.D0 
135 CONTINUE 
ENDIF 

cc wb x (ICM*wb) 

IF(EOM(NB,1)) THEN 
DO 140 1=1,3 
DO 1 50 J=1 ,3 

TEMP_33(I,J) = ICM(NB,I,J) 

150 CONTINUE 
140 CONTINUE 

CALL D_MMUL(TEMP_33, WB, TEMP6_3, 3, 3, 1) 

CALL D_VCROSS(WB, TEMP6_3, TEMP4_3) 

ELSE 

TEMP4_3(1) = 0.D0 
TEMP4_3(2) = 0.D0 
TEMP4_3(3) = 0.D0 
ENDIF 

cc Add nonlinear rotational terms to RHS 
DO 160 1=1,3 

RHS(3+I) = RHS(3+I) - TEMP3_3(I) - TEMP4_3(I) - 
& TEMP5_3(I) 

160 CONTINUE 

cc Nonlinear RHS terms in flex eqns 

cc Rdcmb . (wb x gamO) ; performed on sub vector basis in arrays 
IF( .NOT. EOM(NB,3) ) THEN 
DO 170 M=1,NM 
DO 180 1=1,3 

TEMP2_3(I) = GAM_0(NB,l,M) 

180 CONTINUE 

CALL D_VCROSS(WB, TEMP2_3, TEMP3_3) 
cc Add to RHS 
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RHS(6+M) = RHS(6+M) + D_D0T(TEMP1_3, TEMP3_3, 3) 
170 CONTINUE 
ENDIF 

cc Check for elimination of higher order mass integral terms 
IF(EOM(NB,1)) GOTO 500 

cc HOMITs on RHS of rotational eqns 

cc Compute II (3 x 3); moment of inertia matrix including 

cc effects of HOMITs 

cc Compute IID (derivative of II) (3 x 3) 

DO 200 1=1 ,3 
DO 210 J=1 ,3 

II 1 (l,J) = 0.D0 

ll_2(l,J) = 0.D0 

II 1 D(1,J) = 0.D0 

ll_2D(l,J) = 0.D0 
DO 220 K=1 ,NM 

ll_1 (l,J) = ll_1(l,J) + GSV(NB,7+K)* 

& ( l_1 K(NB,K,I,J) + l_1 K(NB,K,J,I) ) 

II 1 D(I,J) = II 1 D(I,J) + QSV(NB,6+K)* 

& ( l_1 K(NB,K,I,J) + l_1 K(NB,K,J,I) ) 

DO 230 L=1 ,NM 

ll_2(l,J) = II 2(I,J) + 0.5*GSV(NB,7+K)*GSV(NB,7+L)* 

& ( l_2JK(NB,K,L I l,J) + l_2JK(NB,L 1 K,l 1 J) ) 

II 2D(I,J) = II 2D(I,J) + 0.5*QSV(NB,6+K)*GSV(NB,7+L)* 

& ( l_2JK(NB,K,L,l,J) + l_2JK(NB,L.K,l I J) ) + 

& 0.5*GSV(NB,7+K)*QSV(NB,6+L)* 

& ( l_2JK(NB,K,L,l,J) + l_2JK(NB,L,K,l,J) ) 

230 CONTINUE 

220 CONTINUE 

11(1, J) = ICM(NB,I,J) + ll_1(l,J) + II 2(I,J) 

IID(I,J) = II 1 D(I,J) + II 2D(I,J) 

210 CONTINUE 
200 CONTINUE 

cc Compute wb x (II * wb) 

CALL D_MMUL(II, WB, TEMP2_3, 3, 3, 1 ) 

CALL D_VCROSS(WB, TEMP2_3, TEMP1_3) 


cc Compute IID * wb 

CALL D_MMUL(IID, WB, TEMP2_3, 3, 3, 1) 


cc Compute wb x (gam2 * etad) 

DO 240 1=1 ,3 
DO 250 J=1 ,NM 
GAM_2(I,J) = 0.D0 
DO 260 K=1 ,NM 

GAM_2(I,J) = GAM_2(I,J) + GSV(NB,7+K)* 
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& GAM_2JK(NB,K,J,I) 

260 CONTINUE 
250 CONTINUE 
240 CONTINUE 

CALL D_MMUL(GAM_2, TEMP2_MNM, TEMP3_3, 3, MNM, 1) 
CALL D_VCROSS(WB, TEMP3_3, TEMP4_3) 

cc Add rotational HOMITs to RHS 
DO 270 1=1 ,3 

RHS(3+I) = RHS(3+I) - TEMP1_3(I) - TEMP2_3(I) - 
& TEMP4_3(I) 

cc Compute gam_2 dot (3 x NM) for flex eqns 
DO 280 J=1 ,NM 
GAM_2D(I,J) = 0.D0 
DO 290 K=1,NM 

GAM_2D(I,J) = GAM_2D(I,J) + QSV(NB,6+K)* 

& GAM_2JK(NB,K,J,I) 

290 CONTINUE 
280 CONTINUE 
270 CONTINUE 

cc Compute -2*wb . gam_2dot on array sub vector basis 
DO 300 M=1 ,NM 
DO 310 1=1,3 

TEMPI _3(l) = GAM_2D(I,M) 

310 CONTINUE 

RHS(6+M) = RHS(6+M) - 2.D0 * D_DOT(WB, TEMP1_3, 3) 

cc Pull out each l_1k 3x3 submatrix, compute wb'*l1m*wb, 
cc and add to RHS 
DO 320 1=1 ,3 
DO 330 J=1 ,3 

TEMP_33(I,J) = l_1K(NB, M, I, J) 

330 CONTINUE 
320 CONTINUE 

CALL D_MMUL(TEMP_33, WB, TEMP2_3, 3, 3, 1) 

RHS(6+M) = RHS(6+M) + D_DOT(WB, TEMP2_3, 3) 

cc Compute col vect over I: 0.5 * wb' * sumj(l_2jl + l_2lj) * wb 
CALL D_ZERO(TEMP_33, 3, 3) 

DO 340 K=1 ,NM 
DO 350 1=1 ,3 
DO 360 J=1 ,3 

TEMP_33(I,J) = TEMP_33(I,J) + GSV(NB,7+K) * 
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& ( l_2JK(NB,K,M,l,J) + l_2JK(NB,M,K,l,J) ) 

360 CONTINUE 
350 CONTINUE 
340 CONTINUE 

CALL D_MMUL(TEMP_33, WB, TEMP3_3, 3, 3, 1) 

RHS(6+M) = RHS(6+M) + 0.5D0 * D_DOT(WB, TEMP3_3, 3) 

300 CONTINUE 

cc Solve system for deriv of quasi coord vector 
500 CONTINUE 

cc Extract appropriate upper triangular mass matrix 
DO 510 1=1 ,6+NM 
DO 520 J=1 ,6+NM 
MASSM(I.J) = LU_MASSM(NB,I,J) 

520 CONTINUE 
510 CONTINUE 

cccccccccccccccccccccccccccccccccccccccccccc 
c WRITE(6,*) 
c WRITE(6,*) ' RHS: ’ 

c WRITE(6,'(1 X,1 0E1 2.5)') (RHS(I), 1=1 ,6+NM) 
c WRITE(6,*) 

cccccccccccccccccccccccccccccccccccccccccccc 

cc Solve upper triangular system (A*x = B) using back sub 
DO 530 J=1 ,6+NM 
INDXX(J) = INDX(NB,J) 

530 CONTINUE 

CALL LUBKSB(MASSM, 6+NM, 6+MNM, INDXX, RHS) 

cc Store quasi-coord state vector derivative 
DO 550 1=1 ,6+NM 
QSVD(NB.I) = RHS(I) 

550 CONTINUE 

RETURN 

END 
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ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
SUBROUTINE INTEG(NB) 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 


Function: Integrates derivatives of quasi-coord state 

vector (W); transforms (W) to generalized 
coord state vector derivative (qdot) and 
integrates qdot to produce q. Normalizes 
quaternion in generalized coord vector. 

Four integration options based on integer INT: 
INT = 1 ~> Euler or Adams 1 
INT = 2 ~> Adams (3-1)/ 2 
INT = 3 --> Adams (23 - 16 + 5) / 12 


Source: 


JC Date: 2/91 
Double precision 

Data via common block declarations 


cc 

cc Comments: 
cc 
cc 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
IMPLICIT NONE 


cc Include files 

INCLUDE 'BODY.INC' 

cc Local declarations 
INTEGER I 
INTEGER NB, NM 

REAL*8 TEMP1_3(3), TEMP2_3(3), TEMP_4(4), Q4(4) 

REAL‘8 WB(3), ET(4,3), TEMP1_33(3,3), TEMP2_33(3,3) 

cc Initialization 

NM = NMODES(NB) 

cc Transform quasi-coord state vector (QSV) to deriv of gen coord 
cc state vector (GSVD) in sub blocks; {GSVD} = [beta] * {QSV} 

cc Rigid body translation rates 

cc Extract body vel vector PD_B_I_B, ang vel vector W_BJ_B, 
cc and BODY <-- 1 quaternion 
DO 20 1=1,3 

TEMP1_3(I) = QSV(NB.I) 

WB(I) = QSV(NB,3+I) 

Q4(l) = GSV(NB,3+I) 

20 CONTINUE 
04(4) = GSV(NB,7) 

cc Transform PD_B_I_B to PD_B_I_I 
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CALL Q_T_DCM(Q4, TEMPI _33) 

CALL D_MTRANS(TEMP1_33, TEMP2_33, 3, 3) 

CALL D_MMUL(TEMP2_33, TEMPI _3, TEMP2_3, 3, 3, 1) 

cc Build (Q_B_l)dot <-- angular body rate vector trans 
ET(1,1) = Q4(4) 

ET(1,2) = -Q4(3) 

ET(1,3) = Q4(2) 

ET(2,1) = Q4(3) 

ET(2,2) = Q4(4) 

ET(2,3) = -Q4(1) 

ET(3,1) = -Q4(2) 

ET(3,2) = Q4(1) 

ET(3,3) = Q4(4) 

ET(4,1) = -Q4(1) 

ET(4,2) = -04(2) 

ET(4,3) = -Q4(3) 

cc Update Q_B_I derivative 

CALL D_MMUL(ET, WB, TEMP_4, 4, 3, 1) 

cc Add to GSVD 
DO 30 1=1 ,3 

GSVD(NB.I) = TEMP2_3(I) 

GSVD(NB,3+I) = 0.5D0 * TEMP_4(I) 

30 CONTINUE 

GSVD(NB,7) = 0.5D0 * TEMP_4(4) 

cc Modal coord trans is an identity matrix 
DO 50 1=1 ,NM 

GSVD(NB,7+I) = QSV(NB,6+I) 

50 CONTINUE 

cc Integrate gen coord state deriv vector based on INT option 
cc Integrate quasi-coord state deriv vector based on INT option 
IF(INT.EQ. 1) THEN 
DO 60 1=1 ,7+NM 

GSV(NB.I) = GSV(NB.I) + DT*GSVD(NB,I) 

60 CONTINUE 

DO 70 1=1 ,6+NM 

QSV(NB,I) = QSV(NB.I) + DT*QSVD(NB,I) 

70 CONTINUE 

ELSEIF(INT .EQ. 2) THEN 
DO 80 1=1 ,7+NM 

GSV(NB.I) = GSV(NB.I) + (DT/2.D0) * ( 3.D0 * GSVD(NB.I) - 
& GSVDO_1(NB,l) ) 

80 CONTINUE 
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DO 90 1=1 ,6+NM 

QSV(NB.I) = QSV(NB.I) + (DT/2.D0) * ( 3.D0 * QSVD(NB.I) - 
& QSVDO_1 (NB,I) ) 

90 CONTINUE 

ELSEIF(INT .EQ. 3) THEN 
DO 100 1=1 ,7+NM 

GSV(NB.I) = GSV(NB,I) + (DT/12.D0) * ( 23.D0 * GSVD(NB.I) 
& 16.D0 * GSVDO_1(NB,l) + 5.D0 * GSVDO_2(NB,l) ) 

100 CONTINUE 

DO 110 1=1 ,6+NM 

QSV(NB.I) = QSV(NB.I) + (DT/12.D0) * ( 23.D0 * QSVD(NB.I) 
& 16. DO * QSVDO_1 (NB,I) + 5.D0 * QSVDO_2(NB,l) ) 

110 CONTINUE 

ELSE 

WRITE(6,*) '*** INTEG: Integration Option Invalid ***’ 

STOP 

ENDIF 

cc Normalize Q_BJ and replace in generalized-coord vector 
DO 120 1=1,4 
Q4(l) = GSV(NB,3+I) 

120 CONTINUE 

CALL D_QNORM(Q4) 

DO 130 1=1 ,4 
GSV(NB,3+I) = Q4(l) 

130 CONTINUE 

cc Save old state derivative data 
DO 150 1=1 ,7+NM 
GSVDO_2(NB,l) = GSVDO_1(NB,l) 

GSVDO_1 (NB,I) = GSVD(NB.I) 

150 CONTINUE 

DO 160 1=1 ,6+NM 
QSVDO_2(NB,l) = QSVDO_1(NB,l) 

QSVDO_1(NB,l) = QSVD(NB.I) 

160 CONTINUE 

RETURN 

END 
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ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

SUBROUTINE INTFAC 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


cc 

cc Function: 

cc 

cc 

cc Source: 
cc 

cc Comments: 

cc 

cc 


Updates transformations and relative position 
and rate vectors; includes flexibility effects. 

JC Date: 3/91 

Double precision 

Data via common block declarations 


ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


IMPLICIT NONE 


cc Include files 

INCLUDE 'BODY.INC' 
INCLUDE 'INTFAC.INC' 


cc Locals 

INTEGER I, J, N, NB, M 
REAL*8 T_I_B 1(3,3) 

REAL*8 T_D2_D1 (3,3), T_D1 0_B1 (3,3) 

REAL*8 T_D1_D1 0(3,3), T_D20_D2(3,3) 

REAL*8 TEMP1_33(3,3), TEMP2_33(3,3) 

REAL*8 TEMP1_3(3), TEMP2_3(3), TEMP3_3(3), TEMP4_3(3) 
REAL*8 IDENT_33(3,3), Q4(4) 

REAL*8 W_B1_I_B1 (3), W_B2_I_B2(3) 

DATA IDENT_33 / 1 .DO, O.DO, O.DO, O.DO, 1 .DO, O.DO, 

& O.DO, O.DO, 1. DO/ 

cc Build transformations from undeformed docking port systems 
cc to deformed docking point systems (due to infinitesimal 
cc rotations resulting from flexibility). 

DO 40 NB=1 .NBODES 

cc Compute BODY <-- I transformations from generalized coord 
cc vector quaternions 
DO 10 1=1,4 
Q4(l) = GSV(NB,3+I) 

10 CONTINUE 

IF(NB .EQ. 1) THEN 
CALL Q_T_DCM(Q4, T_B1 J) 

ELSEIF(NB .EQ. 2) THEN 
CALL Q_T_DCM(Q4, T_B2_I) 

ENDIF 
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cc Compute flexibility effects at each node 
DO 20 N=1 .NNODES(NB) 

DO 30 J=1 ,3 

PHIETA(NB,N,J) = 0.D0 
PHIETAD(NB,N,J) = 0.D0 
PSIETA(NB,N,J) = 0.D0 
PSIETAD(NB,N,J) = 0.D0 

DO 35 M=1 ,NMODES(NB) 

PHIETA(NB,N,J) = PHIETA(NB,N,J) + PHI(NB,N,M,J)* 

& GSV(NB,7+M) 

PHIETAD(NB,N,J) = PHIETAD(NB,N,J) + PHI(NB,N,M,J)* 
& QSV(NB,6+M) 

PSIETA(NB,N,J) = PSIETA(NB,N,J) + PSI(NB,N,M,J)* 

& GSV(NB,7+M) 

PSIETAD(NB,N,J) = PSIETAD(NB,N,J) + PSKNB.N.M.J)* 
& QSV(NB,6+M) 

35 CONTINUE 
30 CONTINUE 
20 CONTINUE 

cc Compute and add in infinitesimal rotations at DOCKING node 
cc due to flexibility 

TEMPI _3(1) = PSIETA(NB,NODE_DOCK(NB),1 ) 

TEMPI _3(2) = PSIETA(NB,NODE_DOCK(NB),2) 

TEMPI _3(3) = PSIETA(NB,NODE_DOCK(NB),3) 

CALL D_TILDE(TEMP1_3, TEMP1_33) 

IF(NB.EQ. 1) THEN 

CALL D_ADDM(IDENT_33, TEMPI _33, TEMP2_33, 3, 3) 
CALL D_MTRANS(TEMP2_33, T_D1_D10, 3, 3) 
ELSEIF(NB .EQ. 2) THEN 

CALL D_ADDM(IDENT_33, TEMP1_33, T_D20_D2, 3, 3) 
ENDIF 

40 CONTINUE 

cc Compute transformation D1 <-- D2 

CALL D_MTRANS(T_B1_D1 0, T_D10_B1 , 3, 3) 

CALL D_MMUL(T_D1_D1 0, T_D10_B1 , T_D1_B1 , 3, 3, 3) 
CALL D_MT R AN S (T_B 1 _l , T_I_B1 , 3, 3) 

CALL D_MMUL(T_B2_I, T_I_B1 , T_B2_B1 , 3, 3, 3) 

CALL D_MTRANS(T_B2_B1 , T_B1_B2, 3, 3) 

CALL D_M MU L(T_D 1 _B 1 , T_B1_B2, TEMP1_33, 3, 3, 3) 
CALL D_M M U L(T_B 2_D20 , T_D20_D2 t TEMP2_33, 3, 3, 3) 
CALL D_MTRANS(TEMP2_33, T_D2_B2, 3, 3) 

CALL D_MMUL(TEMP1_33, TEMP2_33, T_D1_D2, 3, 3, 3) 
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CALL D_MT R AN S (T_D 1 _D2 , T_D2_D1 , 3, 3) 

cc Compute position vector D2 <-- D1 expressed in D1 frame 
DO 45 1=1 ,3 

TEMPI _3(l) = GSV(2,I) - GSV(1 ,1) 

45 CONTINUE 

CALL D_MXV(T_B1 _l, TEMP1_3, P_B2_B1_B1, 3, 3) 

DO 50 1=1 ,3 

P D1 B1 B1 (I) = P_D10_B1_B1 (I) + PHIETA(1 ,NODE_DOCK(1 ),l) 

TEMPI _3(l) = -P_D1_B1_B1 (I) + P_B2_B1_B1(I) 

50 CONTINUE 

CALL D_MXV(T_D1_B1, TEMP1_3, TEMP2_3, 3, 3) 

DO 60 1=1,3 

P_D2_B2_B2(I) = P_D20_B2_B2(I) + PHIETA(2,NODE_DOCK(2),l) 
60 CONTINUE 

CALL D_MXV (T_D2_B2 , P_D2_B2_B2, TEMP3_3, 3, 3) 

CALL D_MXV(T_D1_D2, TEMP3_3, TEMP1_3, 3, 3) 

DO 70 1=1 ,3 

P_D2_D1_D1 (I) = TEMP2_3(I) + TEMP1_3(I) 

W_B1_I_B1 (l) = QSV(1 ,3+1) 

W_B2_I_B2(I) = QSV(2,3+I) 

TEMP1_3(I) = PSIETA(1 ,NODE_DOCK(1 ),l) 

TEMP2_3(I) = PSIETA(2,NODE_DOCK(2),l) 

70 CONTINUE 

cc Compute angular velocity of D2 wrt D1 expressed in D2 

CALL D_V CROSS (W_B 1 I B1 , TEMPI _3, TEMP3_3) 

CALL D_VCROSS(W_B2_l_B2, TEMP2_3, TEMP4_3) 

DO 80 1=1 ,3 

TEMP1_3(I) = -TEMP3_3(I) - W_B1 J_B1 (0 - 
& PSIETAD(1 ,NODE_DOCK(1 ),l) 

TEMP2_3(I) = TEMP4_3(I) + W_B2_I_B2(I) + 

& PSIETAD(2,NODE_DOCK(2),l) 

80 CONTINUE 

CALL D_MMUL(T_D2_B2, TEMP2_3, TEMP4_3, 3, 3, 3) 

CALL D_MMUL(T_D1_B1 , TEMP1_3, TEMP3_3, 3, 3, 3) 

CALL D_MMUL(T_D2_D1 , TEMP3_3, TEMP1_3, 3, 3, 3) 

DO 90 1=1 ,3 

W_D2_D1_D2(I) = TEMP1_3(I) + TEMP4_3(I) 

90 CONTINUE 
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cc Compute derivative of D2 <-- D1 position vector wrt D1 
cc expressed in D1 

CALL D_VCROSS(W_B1 _I_B1 , P_D1_B1_B1 , TEMP3_3) 
CALL D_VCROSS(W_B2J_B2, P_D2_B2_B2, TEMP4_3) 

DO 110 1=1,3 

TEMPI _3(l) = -TEMP3_3(I) - PHIETAD(1 ,NODE_DOCK(1),l) 
& - QSV(1 ,1) 

TEMP2_3(I) = TEMP4_3(I) + PHIETAD(2,NODE_DOCK(2),l) 

& + QSV(2,I) 

TEMP4_3(I) = PSIETA(1 ,NODE_DOCK(1 ),l) 

110 CONTINUE 

CALL D_MXV (T_D 1 _B 1 , TEMP1_3, TEMP3_3, 3, 3) 

CALL D_MXV (T_D2_B2 , TEMP2_3, TEMPI _3, 3, 3) 

CALL D_MXV(T_D1 _D2, TEMP1_3, TEMP2_3, 3, 3) 

CALL D_V C ROSS (W_B 1 _I_B 1 , TEMP4_3, TEMP1_3) 

DO 120 1=1 ,3 

TEMP4_3(I) = -W_B1 I B1 (I) - PSIETAD(1 ,NODE_DOCK(1),l) 

& - TEMP1_3(I) 

120 CONTINUE 

CALL D_MXV (T_D1 _B 1 , TEMP4_3, TEMPI _3, 3, 3) 

CALL D_VCROSS(TEMP1_3, P_D2_D1_D1 , TEMP4_3) 

DO 130 1=1 ,3 

PD_D2_D1_D1 (I) = TEMP3_3(I) + TEMP2_3(I) + TEMP4_3(I) 
130 CONTINUE 

RETURN 

END 
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APPENDIX B 

MATH MODEL GLOBAL VARIABLE DEFINITIONS 

DT - Integration step size (sec) 

EOM(NBODES,3) - Equations of motion complexity flags 

FN_BODY(NBODES,MNN,3) - External forces applied at each body node 

FS1_S1(3) . Sensed contact forces 

GAM_0(NBODES,3,MNM) - Array of GAM_0 modal integrals 

GAM_1 (NBODES,3,MNM) - Array of GAM_1 modal integrals 

GAM_2JK(NBODES,MNM,MNM,3) - Array of GAM_2JK modal integrals 

GMASS(NBODES,MNM,MNM) - Generalized mass matrices 

GSV(NBODES,7+MNM) - Generalized coordinate vectors 

GSVD(NBODES,7+MNM) - Generalized coordinate vector derivatives 

GSVDO_1 (NBODES.7+MNM) - Previous generalized coordinate vector 

derivatives 

GSVDO_2(NBODES,7+MNM) - Second previous generalized coordinate 

vector derivatives 

ICM(NBODES,3,3) - Body inertia matrices about respective body 

center of mass 

INDX(NBODES,6+MNM) - Record of row permutations in LU 

decomposition 

INT - Integration option flag 

l_1K(NBODES,MNM,3,3) - Array of 3X3 l_1K modal integrals 

l_2JK(NBODES,MNM,MNM,3 1 3) - Array of 3X3 l_2JK modal integrals 

LU_MASSM(NBODES,6+MNM,6+MNM) - LU decomposed mass matrices 
MASS(NBODES) - Body mass 

MN_BODY(NBODES,MNN,3) - External moments applied at each body node 
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MNM 


Maximum number of modes per body 

MNN - Maximum number of nodes per body 

MOD_FREQ(NBODES,MNM) - Flexible mode frequencies 

MOD_ZETA(NBODES,MNM) - Damping per flexible mode 

MS1_S1 (3) - Sensed contact moments 

NBODES - Number of bodies in simulation 

NMODES(NBODES) - Number of flexible modes per body 

NNODES(NBODES) - Number of nodes per body 

NODE_DOCK(NBODES) - Body docking port node number 

NODES(NBODES,MNN,3) - Node locations in respective body coordinates 

P_B2_B1_B1 (3) - Position vector of B2 center of mass with 

respect to B1 center of mass in B1 coordinates 

PD_D2_D1_D1 (3) - Velocity vector of chase docking node with 

respect to target docking node in D1 
coordinates 

p_D1 0_B1 _B1 (3) - Position vector of undeformed target docking 

node with respect to target center of mass in 
B1 coordinates 

P_D1_B1_B1 (3) - Position vector of target docking node with 

respect to target center of mass in B1 
coordinates 

p_Dl_S1_S1 (3) - Position vector of target docking node with 

respect to force/moment sensor in SI 
coordinates 

P_D20_B2_B2(3) - Position vector of undeformed chase docking 

node with respect to chase center of mass in 
B2 coordinates 

P_D2_B2_B2(3) - Position vector of chase docking node with 

respect to chase center of mass in B2 
coordinates 
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P D2_D1_D1(3) - Position vector of chase docking node with 

respect to target docking node in D1 
coordinates 

PHI(NBODES,MNN,MNM,3) - Translational modal gains 

PHIETA(NBODES,MNN,3) - Node translational deformation in respective 

body coordinates 

PHIETAD(NBODES,MNN,3) - Node translational deformation velocity in 

respective body coordinates 

PSI(NBODES,MNN,MNM,3) - Rotational modal gains 

PSIETA(NBODES,MNN,3) - Node rotational deformation in respective 

body coordinates 

PSIETAD(NBODES,MNN,3) - Node rotational deformation velocity in 

respective body coordinates 

QSV(NBODES,6+MNM) - Quasi-coordinate vectors 

QSVD(NBODES,6+MNM) - Quasi-coordinate vector derivatives 

QSVDO_1 (NBODES,6+MNM) - Previous quasi-coordinate vector derivatives 

QSVDO_2(NBODES,6+MNM) - Second previous quasi-coordinate vector 

derivatives 

T_B1_B2(3,3) - Transformation from B2 to B1 coordinates 

T_B1_D10(3,3) - Transformation from undeformed target 

docking frame to B1 coordinates 

j_B 1_|(3,3) - Transformation from inertial to B1 coordinates 

j_B 2_B1(3,3) - Transformation from B1 to B2 coordinates 

T_B2_D20(3,3) - Transformation from undeformed chase 

docking frame to B2 coordinates 

T B2_l(3,3) - Transformation from inertial to B2 coordinates 

T_D1_B1(3,3) - Transformation from B1 to D1 coordinates 

T_D1_D2(3,3) - Transformation from D2 to D1 coordinates 
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T_D1_S1 (3,3) 

T_D2_B2(3,3) 

TIME 

W_D2_D1_D2(3) 


Transformation from force/moment sensor to 
D1 coordinates 

Transformation from B2 to D2 coordinates 
Simulation time (sec) 

Angular velocity of chase docking frame with 
respect to target docking frame in D2 
coordinates 
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APPENDIX C 

FLEXIBLE BODY DATA PRE-PROCESSING CODE 


ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

program massint 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


cc 

cc Purpose: 

cc 

cc 

cc Source: 


Compute mass integral terms for a consistent mass 
matrix. Output modal data for use in flex body sims. 


MC and JC 


cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 

cc 


Explicit Inputs: 
ndof 

nnode 

nmode 

idof_type(ndof) 

inod(ndof) 

mass_mat(ndof,ndof) 
gen_mass(nmode, nmode) 
phi(ndof,nmode) 

node_mat(nnode,3) 

r_offset(3) 

Explicit Outputs: 
gen_mass 
gamma_0(3,nmode) 
gamma_1 (3, nmode) 
gamma_2(nmode,nmode,3) 
l_1 (nmode, 3, 3) 
l_2(nmode,nmode,3,3) 

Others: 
rho(nnode,3) 
a(ndof, nmode) 
a_t(nnode,nmode,3) 

a_a(nnode,nmode,3) 

phi_t(nnode,nmode,3) 

psi(nnode,nmode,3) 


4/91 


number of degrees of freedom (DOF) of 
the system 

number of nodes of the system 
number of modes of the system 
DOF array from DOF map 
node array from DOF map 
consistent mass matrix 
- generalized mass mat (phi'*mass_mat*phi) 
matrix of eigenvectors (mode shapes) of the 
system 

nodal geometry matrix 

offset vector for nodal geometry matrix 


generalized mass matrix (phi’*M*phi) 

zeroth mass integral vector 

first mass integral vector 

second mass integral vector 

first mass integral matrix 

second mass integral matrix 


offset nodal geometry matrix 

matrix of the "aj" vectors, stored column-wise 

subset of the "a" matrix describing the 

translation of the nodes (x,y,z) 

subset of the "a" matrix describing the 

rotation of the nodes (theta_x, theta_y, theta_z) 

subset of the modal matrix describing the 

translation of the nodes (x.y.z) 

subset of the modal matrix describing the 

rotation of the nodes (theta_x, theta_y, theta_z) 
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cc rho_i(3) 
cc 

cc a_t_i(3) 
cc 

cc r_x_a(3) 
cc 

cc a_x_phi(3) 
cc 

cc a_dot_phi 
cc 

cc rho_phi_mat(3,3) 
cc 

cc a_phi_mat(3,3) 
cc 

cc rho_dot_a 
cc zeros(3,3) 
cc 

cc ident(3,3) 
cc 

cc Comments: 


- vector describing the location of a node relative 
to an inertial frame of reference 

- vector of the "a_t" matrix at a particular node 
for a particular node 

- resultant vector of the cross product of "rhoj" 
and "a_tj" 

- resultant vector of the cross product of "a_t_i" 
and "phi_tj" 

- resultant scalar of the dot product of "a_t_i" 
and "phi_t_i" 

- resultant matrix of "rhoj" times "phi_t_i" 
transpose 

- resultant matrix of "aj" times "phi_t_i" 

transpose 

- dot product of "rhoj" and "a_tj" 

- 3x3 matrix of zeros used to initialize the mass 
integral matrices 

- 3x3 identity matrix 


Double precision 
cc 

ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


integer ndofmax, nmodemax, nnodemax 
parameter (ndofmax = 40) 
parameter (nmodemax = 40) 
parameter (nnodemax = 10) 


cc Counters 

integer i, j, k, il , icount, n 


cc Parameters of model 

integer nnode, nmode, ndof 


cc DOF map information 

integer idof_type(ndofmax), inod(ndofmax) 


cc Input variables 

real*8 mass_mat(ndofmax,ndofmax), phi(ndofmax,nmodemax) 
real*8 r_offset(3), node_mat(nnodemax,3) 

real*8 gen_mass(nmodemax, nmodemax), temp(ndofmax, nmodemax) 


cc Offset nodal map variables 

real*8 rho(nnodemax,3), rhoJ(3), rho_x_a(3) 
real*8 rho_phi_mat(3,3), rho_dot_a 


cc Modal matrix variables 

real*8 phi_t(nnodemax, nmodemax, 3), psi(nnodemax, nmodemax, 3) 
real*8 phi_tj(3) 
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cc "A" matrix variables 

real*8 a(ndofmax,nmodemax) 

real*8 a_t(nnodemax, nmodemax, 3), a_a(nnodemax, nmodemax, 3) 
real*8 a_t_i(3), a_x __phi(3), a_dot_phi 
real*8 a_phi_mat(3,3) 

cc Mass integral vectors 

real*8 gamma_0(3, nmodemax), gamma_1 (3,nmodemax) 
real*8 gamma_2(nmodemax, nmodemax, 3) 

cc First mass integral matrix and associated temporary variables 
real*8 l_1_1 (3,3), l_1_2(3,3), M_3(3,3), l_1_4(3,3) 
real*8 l_1 (nmodemax, 3,3), l_1_n_old(3,3) 

cc Second mass integral matrix and associated temporary variables 
real*8 l_2_1 (3,3),l_2_2(3,3),l_2_3(3,3),l_2_4(3,3) 
real*8 l_2(nmodemax,nmodemax,3,3),l_2_n_old(3,3) 

cc 3x3 zero and identity matrices 
real*8 zeros(3,3), ident(3,3) 
character* 1 5 filel , file2 

data zeros / 9*0.d0 / 

data ident/l.dO, 3*0.d0, l.dO, 3*0.d0, l.dO/ 

cc Open data files 
write(6,*) 

write (6, ’(2x," Enter raw data file name: ”,$)') 
read(5,'(a)') filel 

write(6,'(2x, "Enter output data file name: ",$)’) 

read(5,'(a)') file2 

write(6,*) 

open(unit=1 , file = filel , status = 'old', 

& form = 'formatted') 
open(unit=2, file = file2, status = 'unknown', 

& form = 'formatted') 

cc Read DOF map 

read(1,*) nnode.ndof.nmode 

write(6,’(4x,"nnode; ndof; nmode: ”,3i5)') nnode.ndof.nmode 
read(1 ,*) 
do 10 i=1,ndof 
read(1 ,*) inod(i), idof_type(i) 

10 continue 

cc Read nodal geometry matrix 
read(1 ,*) 
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read(1 ,*) (r_offset(i),i=1 ,3) 
do 35 i=1 .nnode 
read(1 ,*) (node_mat(i,j),j=1 ,3) 

35 continue 

cc Read mass matrix 
read(1 ,*) 
do 20 i=1,ndof 

read(1 ,*) (mass_mat(i,j),j=1 ,ndof) 

20 continue 

write(6,*) ’ Mass matrix read complete' 

cc Read modal (eigenvector) matrix 
read(1 ,*) 
do 30 i=1,ndof 
read(1,*) (phi(i,j),j=1,nmode) 

30 continue 

write(6,*) ' Modal matrix read complete’ 

cc Calculate the nodal position vector based on the nodal geometry 
cc matrix and the offset vector 
do 33 i=1 ,nnode 
do 34 j=1 ,3 

rho(i,j) = node_mat(i,j) - r_offset(j) 

34 continue 
33 continue 

cc Calculate generalized mass matrix 

call d_mmul(mass_mat, phi, temp, ndof, ndof, nmode, 

& ndofmax, ndofmax, nmodemax) 

do 36 i=1 , nmode 
do 37 j=1 , nmode 
gen_mass(i,j) = O.dO 
do 38 k=1 ,ndof 

gen_mass(i,j) = gen_mass(i,j) + phi(k,i) * temp(k.j) 

38 continue 
37 continue 

36 continue 

write(6,*) ' Generalized mass matrix formed' 

cc Calculate the "a" matrix 
do 60 j=1 .nmode 
do 40 i=1 ,ndof 
a(i,j) = O.dO 
do 50 k=1 ,ndof 
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a(i,j) = a(i,j) + mass_mat(i,k)*phi(k,j) 

50 continue 
40 continue 
60 continue 

write(6,*) ’ A matrix formed' 

cc Divide the "a" matrix into translational and rotational subsets 
do 80 j=1 .nmode 
do 70 i=1 ,ndof 
if (idof_ty pe (i ) . le . 3) then 
a_t(inod(i), j, idof_type(i)) = a(i,j) 
elseif(idof_type(i).ge.4.and.idof_type(i).le.6) then 
a_a(inod(i), j, (idof_type(i)-3)) = a(i,j) 
endif 

70 continue 
80 continue 

cc Calculate the zeroth mass integral vector 
do 110 j=1, nmode 
do 100 k=1,3 
gamma_0(k,j) = O.OdO 
do 90 i=1 ,nnode 

gamma_0(k,j) = gamma_0(k,j) + a_t(i,j,k) 

90 continue 
100 continue 
110 continue 

write(6,*) ' Gamma_0J calculated' 

cc Calculate the first mass integral vector 
do 150 j=1 .nmode 
do 130 k=1 ,3 
gamma_1 (k,j) = O.OdO 
do 140 i=1,nnode 
do 141 n=1 ,3 
a_t_i(n) = a_t(i,j,n) 
rho_i(n) = rho(i.n) 

141 continue 

call d_vcross( rhoj, a_t_i, rho_x_a ) 

gamma_1 (k.j) = gamma_1 (k,j) + a_a(i,j,k) + rho_x_a(k) 

140 continue 
130 continue 
150 continue 

write(6,*) ' Gammo_1J calculated' 

cc Divide the modal matrix into translational and rotational subsets 
do 170 j=1 .nmode 
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do 160 i=1,ndof 
if(idof_type(i).le.3) then 
phi_t( inod(i), j, idof_type(i) ) = phi(ij) 
elseif(idof_type(i).ge.4.and.idof_type(i).le.6) then 
psi( inod(i), j, (idof_type(i)-3) ) = phi(i.j) 
endif 

160 continue 
170 continue 

write(6,*) ’ Separation of trans and rot modes' 

cc Initialize counter 
icount = 1 

cc Calculate the second mass integral vector 
do 230 j=icount,nmode 
do 220 k=icount,nmode 
do 180 i=1,3 
gamma_2(j,k I i) = O.OdO 

180 continue 

do 210 il = l.nnode 
do 181 i-1,3 
a_tj(i) = ,j»0 

phi_t_i(i) = phi_t(i1 ,k,i) 

181 continue 

call d_vcross( a_t_i, phi_tj, a_x_phi ) 
do 200 i=1 ,3 

gamma_2G,k,i) = gamma_2(j,k,i) + a_x_phi(i) 
gamma_2(k,j,i) = -gamma_2(j,k,i) 

200 continue 

210 continue 

220 continue 

icount = icount+1 
230 continue 

write(6,*) ' Gamma_2JK calculated' 

cc Calculate the first mass integral matrix 
do 280 k=1,nmode 

call d_fmim( l_1 , zeros, k, nmodemax ) 
do 270 j=1 ,nnode 

call d_mf3dm( l_1 , l_1_n_old, k, nmodemax ) 

call d_vf3dm( a_t, a_t_i, j, k, nnodemax, nmodemax ) 

call d_vf3dm( phi_t, phi_t_i, j, k, nnodemax, nmodemax ) 

call d_vf2dm( rho, rhoj, j, nnodemax ) 

call d_dot( rhoj, a_t_i, rho_dot_a ) 

call d_cxm( ident, rho_dot_a, l_1_1 , 3, 3 ) 

call d_mmul( phi_t_i, rhoj, rho_phi_mat, 3, 1,3, 3, 1,3) 

call d_chsn( rho_phi_mat, l_1_2, 3, 3 ) 
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call d_addm( l_1_1 , l_1_ 2, l_1_3, 3, 3 ) 
call d_addm( l_1_3, l_1_n_old, l_1_4, 3, 3) 
call d_fmim( l_1 , l_1_4, k, nmodemax ) 

270 continue 
280 continue 

write(6,*) ’ l_1 K calculated’ 

cc Calculate the second mass integral matrix 
do 310 j=1,nmode 
do 300 k=1 , nmode 

call d_smim( I 2, zeros, j, k, nmodemax ) 

do 290 i=1 ,nnode 

call d_mf4dm( l_2, l_2_n_old, j, k, nmodemax ) 
call d_vf3dm( a_t, a_t_i, i, j, nnodemax, nmodemax ) 
call d_vf3dm( phi_t, phi_t_i, i, k, nnodemax, 

& nmodemax ) 

call d_dot( a_t_i, phi_t_i, a_dot_phi ) 

call d_cxm( ident, a_dot_phi, l_2_1 , 3, 3 ) 

call d_mmul( a_t_i, phi_t_i, a_phi_mat, 3, 1 , 3, 3, 1 , 3) 

call d_chsn( a_phi_mat, l_2_2, 3, 3 ) 

call d_addm( l_2_1 , l_2_2, l_2_3, 3, 3 ) 

call d_addm( l_2_3, l_2_n_old, l_2_4, 3, 3 ) 

call d_smim( l_2, l_2_4, j, k, nmodemax ) 

290 continue 
300 continue 
310 continue 

write(6,*) ' l_2JK calculated’ 
write(6,*) 

cc Write output in format consistent with NAMTB flex body 
cc simulation input 

write(6,'(2x, "Writing output data to: ”,a15)') file2 
write(6,*) 

write(6,*) ' Generalized mass matrix' 
do i=1 .nmode 

write(2,‘) (gen_mass(i,j),j=1 , nmode) 
enddo 

write(6,*) ' Trans and rot modal gains’ 
do i=1 .nnode 
do j=1 , nmode 

write(2,500) (phi_t(i,j,k),k=1 ,3) 
enddo 
enddo 
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do i=1,nnode 
do j=1 ,nmode 

write(2,500) (psi(i,j,k),k=1,3) 
enddo 
enddo 

write(6,*) ’ Gamma_0J' 
do j=1,nmode 

write(2,500) (gamma_0(i,j),i=1 ,3) 
enddo 

write(6,*) ' Gamma_1J' 
do j=1 ,nmode 

write(2,500) (gamma_1 (i,j),i=1 ,3) 
enddo 

write(6,*) ' Gamma_2JK' 
do i=1 ,nmode 
do j=1 ,nmode 

write(2,500) (gamma_2(i,j,k),k=1 ,3) 
enddo 
enddo 

write(6,T l_1K' 
do i=1 ,nmode 
do j=1 ,3 

write(2,500) (l_1(i,j,k),k=1,3) 
enddo 
enddo 

write(6,*) 1 l_2JK' 
do i=1,nmode 
do i1=1,nmode 
do j=1 ,3 

write(2,500) (I_2(i,i1 ,j,k),k=1 ,3) 
enddo 
enddo 
enddo 

write(6,*) 

500 format(1x,3e15.7) 

close(9) 

close(l) 

stop 

end 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
subroutine d_fmim(a,b,k,n) 


1 34.0691. FH 


107 



cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

cc 

cc Function: Sets the value of the first mass integral matrix 

cc 

cc Source: MC 

cc 

cc Explicit Inputs: 
cc b - 3x3 matrix 

cc n - number of modes 

cc k - mode counter 

cc 

cc Explicit Outputs: 

cc a - first mass integral matrix nx3x3 

cc 

cc Comments: Double precision 

cc 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

integer n,i,j,k 
real*8 a(n,3,3), b(3,3) 

do 2 i=1 ,3 
do 1 j=1 ,3 
a(k,i,j) = b(i,j) 

1 continue 

2 continue 

return 

end 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
subroutine d_smim(a,b,k1 ,k2,n) 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

cc 

cc Function: Sets the value of the second mass integral matrix 

cc 

cc Source: MC 

cc 

cc Explicit Inputs: 
cc b - 3x3 matrix 

cc n - number of modes 

cc kl - mode counter 1 

cc k2 - mode counter 2 

cc 

cc Explicit Outputs: 

cc a - second mass integral matrix nxnx3x3 

cc 

cc Comments: Double precision 

cc 
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cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

integer n, i, j, kl , k2 
real*8 a(n,n,3,3), b(3,3) 

do 2 i=1 ,3 
do 1 j=1 ,3 
a(k1,k2,i,j) = b(i,j) 

1 continue 

2 continue 

return 

end 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
subroutine d_mf4dm( a,b,i,j,n ) 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

cc 

cc Function: Separates a 3x3 matrix from a 4D matrix 

cc 

cc Source: MC 

cc 

cc Explicit Inputs: 

cc a - 4-dimensional matrix nxnx3x3 

cc i - row position 

cc n - row dimension 

cc 

cc Explicit Outputs: 

cc b - 3x3 matrix 

cc 

cc Comments: Double precision 

cc 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

integer n,i,j,k,k1 
real*8 a(n,n,3,3), b(3,3) 

do 2 k=1 ,3 
do 1 k1=1,3 
b(k,k1) = a(i,j,k,k1) 

1 continue 

2 continue 

return 

end 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
subroutine d_mf3dm( a,b,i,n ) 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
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cc 

cc Function: 
cc 

cc Source: 
cc 

cc Explicit Inputs: 

cc 

cc 

cc 

cc 

cc Explicit Outputs: 

cc 

cc 

cc Comments: 
cc 


Separates a 3x3 matrix from a 3D matrix 
MC 


a - 3-dimensional matrix nx3x3 
i - row position 
n - row dimension 


b - 3x3 matrix 
Double precision 


cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


integer n,i,j,k 
real*8 a(n,3,3), b(3,3) 

do 2 k=1 ,3 
do 1 j=1 ,3 
bG.k) = a(i,j,k) 

1 continue 

2 continue 


return 

end 


cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
subroutine d_vf3dm( a,b,i j,n,m ) 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


cc 

cc Function: Separates a 3 element vector from a 3D matrix 

cc 


cc Source: 
cc 

cc Explicit Inputs: 

cc 

cc 

cc 

cc 

cc 

cc 

cc Explicit Outputs: 

cc 

cc 

cc Comments: 
cc 


MC 


a - 3-dimensional matrix nxmx3 
i - row position 
j - column position 
n - row dimension 
m - column dimension 


b - three element vector 
Double precision 
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cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

integer n,m,i,j,k 
real*8 a(n,m,3), b(3) 

do 1 k=1 ,3 
b(k) = a(i,j,k) 

1 continue 

return 

end 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
subroutine d_vf2dm( a,b,i,n ) 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

cc 

cc Function: Separates a 3 element vector from a 2D matrix 

cc 

cc Source: MC 

cc 

cc Explicit Inputs: 

cc a - 2-dimensional matrix nx3 

cc i - row position 

cc n - row dimension 

cc 

cc Explicit Outputs: 

cc b - three element vector 

cc 

cc Comments: Double precision 

cc 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

integer n,i,k 
real*8 a(n,3), b(3) 

do 1 k=1 ,3 
b(k) = a(i,k) 

1 continue 

return 

end 
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